├── .gitignore ├── obgraphic ├── README ├── Obvious.h ├── CMakeLists.txt └── Obvious2DMap.cpp ├── obvision ├── README ├── reconstruct │ ├── reconstruct_defs.h │ ├── grid │ │ ├── RayCastAxisAligned2D.h │ │ ├── TsdGridBranch.h │ │ ├── RayCastPolar2D.h │ │ └── TsdGridComponent.h │ └── space │ │ ├── RayCastAxisAligned3D.h │ │ ├── TsdSpaceBranch.h │ │ ├── TsdSpaceComponent.h │ │ ├── SensorPolar3D.h │ │ ├── SensorProjective3D.h │ │ └── RayCast3D.h ├── registration │ ├── icp │ │ ├── assign │ │ │ ├── filter │ │ │ │ ├── IPreAssignmentFilter.h │ │ │ │ ├── OutOfBoundsFilter2D.cpp │ │ │ │ ├── ReciprocalFilter.h │ │ │ │ ├── OutOfBoundsFilter2D.h │ │ │ │ ├── OcclusionFilter.h │ │ │ │ ├── OutOfBoundsFilter3D.h │ │ │ │ ├── TrimmedFilter.h │ │ │ │ ├── DistanceFilter.h │ │ │ │ ├── ProjectionFilter.h │ │ │ │ ├── IPostAssignmentFilter.h │ │ │ │ ├── OutOfBoundsFilter3D.cpp │ │ │ │ ├── RobotFootprintFilter.cpp │ │ │ │ ├── DistanceFilter.cpp │ │ │ │ ├── RobotFootprintFilter.h │ │ │ │ ├── TrimmedFilter.cpp │ │ │ │ ├── OcclusionFilter.cpp │ │ │ │ └── ReciprocalFilter.cpp │ │ │ ├── NaboPairAssignment.h │ │ │ ├── assignbase.h │ │ │ ├── AnnPairAssignment.cpp │ │ │ ├── NaboPairAssignment.cpp │ │ │ ├── ProjectivePairAssignment.h │ │ │ ├── FlannPairAssignment.h │ │ │ └── AnnPairAssignment.h │ │ ├── icp_def.h │ │ ├── IcpMultiInitIterator.h │ │ ├── IRigidEstimator.h │ │ ├── IcpMultiInitIterator.cpp │ │ ├── PointToLineEstimator2D.h │ │ ├── ClosedFormEstimator2D.h │ │ ├── PointToPlaneEstimator3D.h │ │ └── PointToPointEstimator3D.h │ ├── amcl │ │ └── AdaptiveMonteCarloMatching.h │ └── ransacMatching │ │ └── RandomMatching.h ├── ransac │ └── RansacPrimitives.h ├── normals │ └── NormalsEstimator.h └── planning │ ├── AStar.h │ ├── Obstacle.h │ ├── AStarNode.cpp │ └── AStarNode.h ├── test ├── gtest-1.7.0.zip ├── README.md └── obcore │ ├── base │ └── pointcloud.cpp │ └── CMakeLists.txt ├── applications ├── lua │ ├── function.lua │ ├── functionWithCallback.lua │ ├── config.lua │ ├── statePa.lua │ ├── statePi.lua │ └── statePo.lua ├── logging_example.cpp ├── uvccam_querycapabilities.cpp ├── obvious3D_show.cpp ├── uvccam_finddevice.cpp ├── kinect.xml ├── showCloud.cpp ├── xtion.xml ├── lua_call_function.cpp ├── statemachine_test.cpp ├── kinect_ir.xml ├── ransac_circle.cpp ├── lua_callback_c.cpp ├── ndt_matching2D.cpp ├── lua_statemachine.cpp ├── kinect_perspective.cpp ├── obvious3D_map.cpp ├── kinect_playback.cpp ├── lua_read_config.cpp ├── xtionStream.cpp ├── uvcvirtualcam_serialize.cpp ├── nanoStream.cpp └── uvccam_serialize.cpp ├── obcore ├── statemachine │ ├── AgentModel.cpp │ ├── states │ │ ├── StateBase.cpp │ │ ├── StatePing.cpp │ │ ├── StatePong.cpp │ │ ├── StatePing.h │ │ ├── StatePong.h │ │ ├── StateBaseModel.h │ │ ├── StateBase.h │ │ ├── StateLua.h │ │ └── StateLua.cpp │ ├── AgentModel.h │ ├── Agent.h │ └── RobotModel.cpp ├── math │ ├── linalg │ │ ├── linalg.h.in │ │ ├── eigen │ │ │ ├── Vector.cpp │ │ │ └── Vector.h │ │ ├── gsl │ │ │ ├── Vector.cpp │ │ │ └── Vector.h │ │ └── MatrixFactory.h │ ├── geometry.h │ ├── Trajectory.cpp │ ├── Trajectory.h │ ├── IntegratorSimpson.cpp │ ├── PID_Controller.cpp │ └── IntegratorSimpson.h ├── Axis.h ├── base │ ├── Timer.cpp │ ├── CartesianCloudFactory.h │ ├── tools.h │ ├── types.h │ ├── Point.h │ ├── System.inl │ ├── Time.cpp │ ├── CartesianCloudFactory.cpp │ └── System.h ├── filter │ ├── EuclideanFilter.h │ ├── EuclideanFilterVecD.cpp │ ├── CartesianFilter.h │ ├── FilterDistance.h │ ├── EuclideanFilterVecD.h │ ├── CartesianFilter.cpp │ ├── EuclideanFilter.cpp │ ├── BoundingBoxFilter.h │ └── NormalFilter.cpp ├── grid │ ├── GradientGrid.h │ ├── HeightGrid.h │ └── HeightGrid.cpp ├── scripting │ └── LuaScriptManager.h └── CMakeLists.txt ├── installObviously ├── README.md ├── obdevice ├── PclCloudInterface.h ├── ParentDevice3D.cpp ├── OpenNiDevice.h ├── CloudFactory.h ├── SickLMS100.h ├── UvcVirtualCam.h ├── KinectPlayback.h ├── CMakeLists.txt └── ParentDevice3D.h └── License.txt /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /obgraphic/README: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /obvision/README: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /test/gtest-1.7.0.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/autonohm/obviously/HEAD/test/gtest-1.7.0.zip -------------------------------------------------------------------------------- /applications/lua/function.lua: -------------------------------------------------------------------------------- 1 | -- This is an example of using Lua functions 2 | function aLuaFunction (x, y) 3 | return (x^2 * math.sin(y))/(1 - x) 4 | end 5 | -------------------------------------------------------------------------------- /applications/lua/functionWithCallback.lua: -------------------------------------------------------------------------------- 1 | -- This is an example of using Lua functions 2 | function aLuaFunctionWithCallback (x) 3 | return mysin(x) 4 | end 5 | -------------------------------------------------------------------------------- /test/README.md: -------------------------------------------------------------------------------- 1 | Using gtest with obviously 2 | -------------------------- 3 | $ unzip gtest-1.7.0.zip 4 | $ mv gtest-1.7.0 obcore 5 | $ cd obcore/gtest-1.7.0 6 | $ cmake . 7 | $ make 8 | $ cd ../build 9 | $ make -------------------------------------------------------------------------------- /applications/lua/config.lua: -------------------------------------------------------------------------------- 1 | -- This is an example of using Lua tables 2 | group1 = { value1 = 0.17, 3 | value2 = 1.2 } 4 | 5 | group2 = { value3 = 1, 6 | value4 = false, 7 | value5 = "Greetings from Lua" } 8 | -------------------------------------------------------------------------------- /obcore/statemachine/AgentModel.cpp: -------------------------------------------------------------------------------- 1 | #include "AgentModel.h" 2 | #include 3 | 4 | namespace obvious 5 | { 6 | 7 | AgentModel::AgentModel() 8 | { 9 | 10 | } 11 | 12 | AgentModel::~AgentModel() 13 | { 14 | 15 | } 16 | 17 | } // end namespace 18 | 19 | -------------------------------------------------------------------------------- /obcore/math/linalg/linalg.h.in: -------------------------------------------------------------------------------- 1 | /** 2 | * linalg.h is generated through cmake and linalg.h.in 3 | */ 4 | #if ${USE_EIGEN} 5 | #include "eigen/Matrix.h" 6 | #include "eigen/Vector.h" 7 | #else 8 | #include "gsl/Matrix.h" 9 | #include "gsl/Vector.h" 10 | #endif 11 | #include "MatrixFactory.h" 12 | -------------------------------------------------------------------------------- /obvision/reconstruct/reconstruct_defs.h: -------------------------------------------------------------------------------- 1 | #include "obcore/base/types.h" 2 | 3 | #if _OBVIOUS_DOUBLE_PRECISION_ 4 | #define TSDGRIDMAXWEIGHT 32.0 5 | #define TSDSPACEMAXWEIGHT 32.0 6 | #define TSDINC 1.0 7 | #define TSDZERO 0.0 8 | #else 9 | #define TSDGRIDMAXWEIGHT 32.f 10 | #define TSDSPACEMAXWEIGHT 32.f 11 | #define TSDINC 1.f 12 | #define TSDZERO 0.f 13 | #endif 14 | -------------------------------------------------------------------------------- /obcore/Axis.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Axis.h 3 | * @author Christian Pfitzner 4 | * @date 14.12.2012 5 | * 6 | * 7 | */ 8 | 9 | #ifndef AXIS_H_ 10 | #define AXIS_H_ 11 | 12 | namespace obvious 13 | { 14 | /** 15 | * @enum Axis 16 | */ 17 | enum Axis 18 | { 19 | X, //!< X x axis 20 | Y, //!< Y y axis 21 | Z //!< Z z axis 22 | }; 23 | }; // 24 | 25 | #endif /* AXIS_H_ */ 26 | -------------------------------------------------------------------------------- /obcore/base/Timer.cpp: -------------------------------------------------------------------------------- 1 | #include "Timer.h" 2 | 3 | namespace obvious { 4 | 5 | void Timer::start(void) 6 | { 7 | _start = Time::now(); 8 | } 9 | 10 | double Timer::reset(void) 11 | { 12 | Time old(_start); 13 | _start = Time::now(); 14 | return _start - old; 15 | } 16 | 17 | double Timer::elapsed(void) const 18 | { 19 | return Time::now() - _start; 20 | } 21 | 22 | } // end namespace obvious 23 | -------------------------------------------------------------------------------- /applications/logging_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "obcore/base/Logger.h" 3 | 4 | using namespace std; 5 | using namespace obvious; 6 | 7 | int main(int argc, char* argv[]) 8 | { 9 | LOGMSG_CONF("logging_example", Logger::file_on|Logger::screen_on, DBG_DEBUG, DBG_ERROR); 10 | LOGMSG(DBG_DEBUG, "hello " << "world"); 11 | LOGMSG(DBG_WARN, "something " << "strange"); 12 | LOGMSG(DBG_ERROR, "this is an error"); 13 | } 14 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StateBase.cpp: -------------------------------------------------------------------------------- 1 | #include "StateBase.h" 2 | #include 3 | 4 | namespace obvious 5 | { 6 | 7 | StateBase::StateBase() 8 | { 9 | _agent = NULL; 10 | } 11 | 12 | StateBase::~StateBase() 13 | { 14 | 15 | } 16 | 17 | void StateBase::setAgent(Agent* agent) 18 | { 19 | _agent = agent; 20 | } 21 | 22 | Agent* StateBase::getAgent() 23 | { 24 | return _agent; 25 | } 26 | 27 | 28 | } /* namespace obvious */ 29 | 30 | -------------------------------------------------------------------------------- /obcore/math/geometry.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_H_ 2 | #define GEOMETRY_H_ 3 | #include "obcore/base/CartesianCloud.h" 4 | #include "obcore/math/linalg/linalg.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | void calculatePerspective(Matrix* P, CartesianCloud3D* cloud, int nW, int nH, int subsample=1); 10 | void calculatePerspective2(Matrix* P, CartesianCloud3D* cloud, int nW, int nH, int subsample=1); 11 | 12 | bool axisAngle(Matrix M, double* axis, double* angle); 13 | 14 | } 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /applications/lua/statePa.lua: -------------------------------------------------------------------------------- 1 | -- This is an example of using Lua statemachines 2 | _agentID = -1 3 | 4 | function onLoad (agentID) 5 | print "### load Pa state script" 6 | _agentID = agentID 7 | end 8 | 9 | function onEntry () 10 | print "--> enter Pa state" 11 | end 12 | 13 | function onActive () 14 | print " activated Pa state" 15 | stateID = 3; 16 | transitionToPersistantState(_agentID, stateID); 17 | return 0 18 | end 19 | 20 | function onExit () 21 | print "<-- exit Pa state" 22 | print "" 23 | end 24 | -------------------------------------------------------------------------------- /applications/lua/statePi.lua: -------------------------------------------------------------------------------- 1 | -- This is an example of using Lua statemachines 2 | _agentID = -1 3 | 4 | function onLoad (agentID) 5 | print "### load Pi state script" 6 | _agentID = agentID 7 | end 8 | 9 | function onEntry () 10 | print "--> enter Pi state" 11 | end 12 | 13 | function onActive () 14 | print " activated Pi state" 15 | stateID = 2; 16 | transitionToPersistantState(_agentID, stateID); 17 | return 0 18 | end 19 | 20 | function onExit () 21 | print "<-- exit Pi state" 22 | print "" 23 | end 24 | -------------------------------------------------------------------------------- /applications/lua/statePo.lua: -------------------------------------------------------------------------------- 1 | -- This is an example of using Lua statemachines 2 | _agentID = -1 3 | 4 | function onLoad (agentID) 5 | print "### load Po state script" 6 | _agentID = agentID 7 | end 8 | 9 | function onEntry () 10 | print "--> enter Po state" 11 | end 12 | 13 | function onActive () 14 | print " activated Po state" 15 | stateID = 1; 16 | transitionToPersistantState(_agentID, stateID); 17 | return 0 18 | end 19 | 20 | function onExit () 21 | print "<-- exit Po state" 22 | print "" 23 | end 24 | -------------------------------------------------------------------------------- /obcore/math/Trajectory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Trajectory.cpp 3 | * 4 | * Created on: 30.01.2014 5 | * Author: user 6 | */ 7 | 8 | #include "Trajectory.h" 9 | 10 | using namespace obvious; 11 | 12 | Trajectory::Trajectory() 13 | { 14 | 15 | } 16 | 17 | Trajectory::~Trajectory() 18 | { 19 | 20 | } 21 | 22 | void obvious::Trajectory::setPose(const Matrix pose) 23 | { 24 | _trajectory.push_back(pose); 25 | } 26 | 27 | std::vector obvious::Trajectory::getTrajectory(void) const 28 | { 29 | return _trajectory; 30 | } 31 | 32 | 33 | -------------------------------------------------------------------------------- /obvision/reconstruct/grid/RayCastAxisAligned2D.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCASTAXISALIGNED2D_H_ 2 | #define RAYCASTAXISALIGNED2D_H_ 3 | 4 | #include "obcore/base/types.h" 5 | #include "obvision/reconstruct/grid/TsdGrid.h" 6 | 7 | namespace obvious { 8 | 9 | class RayCastAxisAligned2D { 10 | public: 11 | RayCastAxisAligned2D(); 12 | virtual ~RayCastAxisAligned2D(); 13 | void calcCoords(TsdGrid* grid, obfloat* coords, obfloat* normals, unsigned int* cnt, char* occupiedGrid = NULL); 14 | }; 15 | 16 | } 17 | 18 | #endif /* RAYCASTAXISALIGNED2D_H_ */ 19 | -------------------------------------------------------------------------------- /obvision/reconstruct/space/RayCastAxisAligned3D.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCASTAXISALIGNED3D_H_ 2 | #define RAYCASTAXISALIGNED3D_H_ 3 | 4 | #include "obvision/reconstruct/space/TsdSpace.h" 5 | 6 | namespace obvious { 7 | 8 | class RayCastAxisAligned3D { 9 | public: 10 | RayCastAxisAligned3D(); 11 | 12 | virtual ~RayCastAxisAligned3D(); 13 | 14 | void calcCoords(TsdSpace* space, obfloat* coords, obfloat* normals, unsigned char* rgb, unsigned int* cnt); 15 | 16 | void calcCoordsRoughly(TsdSpace* space, obfloat* coords, obfloat* normals, unsigned int* cnt); 17 | }; 18 | 19 | } 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /obvision/reconstruct/grid/TsdGridBranch.h: -------------------------------------------------------------------------------- 1 | #ifndef TSDGRIDBRANCH_H 2 | #define TSDGRIDBRANCH_H 3 | 4 | #include "obvision/reconstruct/grid/TsdGridComponent.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | class TsdGridBranch : public TsdGridComponent 10 | { 11 | public: 12 | TsdGridBranch(TsdGridComponent*** leafs, int x, int y, int level); 13 | 14 | virtual ~TsdGridBranch(); 15 | 16 | vector getChildren(); 17 | 18 | virtual void increaseEmptiness(); 19 | 20 | void print(); 21 | 22 | void printEdges(); 23 | 24 | private: 25 | 26 | vector _children; 27 | 28 | }; 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /applications/uvccam_querycapabilities.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "obdevice/UvcCam.h" 3 | #include "obcore/base/tools.h" 4 | 5 | using namespace std; 6 | using namespace obvious; 7 | 8 | int main(int argc, char* argv[]) 9 | { 10 | char* dev = (char*)"/dev/video0"; 11 | if(argc>1) dev = argv[1]; 12 | 13 | char serial[] = "8A8B4C0F"; 14 | char* path = NULL; 15 | UvcCam::FindDevice(serial, path); 16 | if(path==NULL) 17 | { 18 | return -1; 19 | } 20 | cout << path << endl; 21 | 22 | UvcCam* cam = new UvcCam(dev, 640, 480); 23 | 24 | cam->printAvailableFormats(); 25 | 26 | delete path; 27 | delete cam; 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /obvision/reconstruct/space/TsdSpaceBranch.h: -------------------------------------------------------------------------------- 1 | #ifndef TSDSPACEBRANCH_H 2 | #define TSDSPACEBRANCH_H 3 | 4 | #include "obvision/reconstruct/space/TsdSpaceComponent.h" 5 | #include 6 | 7 | namespace obvious 8 | { 9 | 10 | class TsdSpaceBranch : public TsdSpaceComponent 11 | { 12 | public: 13 | TsdSpaceBranch(TsdSpaceComponent**** leafs, int x, int y, int z, int level); 14 | 15 | virtual ~TsdSpaceBranch(); 16 | 17 | std::vector getChildren(); 18 | 19 | virtual void increaseEmptiness(); 20 | 21 | void print(); 22 | 23 | void printEdges(); 24 | 25 | private: 26 | 27 | std::vector _children; 28 | 29 | }; 30 | 31 | } 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /obcore/base/CartesianCloudFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef CARTESIANCLOUDFACTORY_H 2 | #define CARTESIANCLOUDFACTORY_H 3 | 4 | #include "obcore/base/CartesianCloud.h" 5 | 6 | using namespace std; 7 | 8 | /** 9 | * @namespace obvious 10 | */ 11 | namespace obvious 12 | { 13 | 14 | enum EnumFileFormat { eFormatAscii }; 15 | 16 | /** 17 | * @class CartesianCloudFactory 18 | * @brief 19 | * @author Stefan May 20 | **/ 21 | class CartesianCloudFactory 22 | { 23 | public: 24 | 25 | static CartesianCloud3D* load(char* filename, EnumFileFormat format); 26 | 27 | static void serialize(char* filename, CartesianCloud3D* cloud, EnumFileFormat format); 28 | }; 29 | 30 | } 31 | 32 | #endif /*CARTESIANCLOUDFACTORY_H*/ 33 | -------------------------------------------------------------------------------- /applications/obvious3D_show.cpp: -------------------------------------------------------------------------------- 1 | #include "obgraphic/Obvious3D.h" 2 | #include 3 | 4 | using namespace obvious; 5 | using namespace std; 6 | 7 | int main(int argc, char *argv[]) 8 | { 9 | VtkCloud* cloud = NULL; 10 | 11 | /** 12 | * Try to determine file type by extension 13 | */ 14 | if(argc>1) 15 | cloud = VtkCloud::load(argv[1], VTKCloud_AUTO); 16 | 17 | /** 18 | * If file was not specified or could not be loaded, show example 19 | */ 20 | if(cloud == NULL) 21 | cloud = VtkCloud::createExample(); 22 | 23 | Obvious3D* viewer = new Obvious3D(); 24 | 25 | viewer->addCloud(cloud); 26 | viewer->startRendering(); 27 | 28 | delete viewer; 29 | delete cloud; 30 | } 31 | -------------------------------------------------------------------------------- /obcore/base/tools.h: -------------------------------------------------------------------------------- 1 | #ifndef TOOLS_H__ 2 | #define TOOLS_H__ 3 | 4 | #include 5 | 6 | namespace obvious 7 | { 8 | 9 | void rgb2gray(unsigned char* src, unsigned char* dst, unsigned int width, unsigned int height); 10 | 11 | int serializePPM(const char* szFilename, void* pvBuffer, unsigned int nWidth, unsigned int nHeight, bool bInc = 1); 12 | 13 | int serializePGM(const char* szFilename, void* pvBuffer, unsigned int nWidth, unsigned int nHeight, bool bInc); 14 | 15 | int serializePBM(const char* szFilename, void* pvBuffer, unsigned int nWidth, unsigned int nHeight, bool bInc = 1); 16 | 17 | double getDoubleLine(std::istream& source); 18 | 19 | int getIntLine(std::istream& source); 20 | 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /applications/uvccam_finddevice.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "obdevice/UvcCam.h" 3 | #include "obcore/base/tools.h" 4 | 5 | using namespace std; 6 | using namespace obvious; 7 | 8 | /** 9 | * In order to find out the serial number of your uvc cam use: 10 | * udevadm info -a -p $(udevadm info -q path -p /class/video4linux/video0) 11 | */ 12 | int main(int argc, char* argv[]) 13 | { 14 | if(argc!=2) 15 | { 16 | cout << "usage " << argv[0] << " serial#" << endl; 17 | return -1; 18 | } 19 | 20 | char* path = NULL; 21 | UvcCam::FindDevice(argv[1], path); 22 | if(path==NULL) 23 | { 24 | cout << "Device could not be found" << endl; 25 | return -1; 26 | } 27 | cout << path << endl; 28 | delete path; 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /obcore/statemachine/AgentModel.h: -------------------------------------------------------------------------------- 1 | #ifndef AGENTMODEL_H_ 2 | #define AGENTMODEL_H_ 3 | 4 | #include "obcore/statemachine/Agent.h" 5 | #include "obcore/base/Point.h" 6 | #include "obcore/base/Timer.h" 7 | 8 | #include 9 | 10 | /** 11 | * @namespace obvious 12 | */ 13 | namespace obvious 14 | { 15 | 16 | /** 17 | * @class AgentModel 18 | * @brief Generic class for agent models. Use this as template for your concrete model. 19 | * @author Stefan May 20 | * @date 17.6.2015 21 | */ 22 | class AgentModel 23 | { 24 | 25 | public: 26 | 27 | /** 28 | * Constructor 29 | */ 30 | AgentModel(); 31 | 32 | /** 33 | * Destructor 34 | */ 35 | virtual ~AgentModel(); 36 | 37 | private: 38 | 39 | }; 40 | 41 | } // end namespace 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StatePing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "obcore/statemachine/Agent.h" 5 | #include "StatePing.h" 6 | #include "StatePong.h" 7 | 8 | namespace obvious 9 | { 10 | 11 | StatePing::StatePing(AgentModel* model) : StateBaseModel(model) 12 | { 13 | } 14 | 15 | StatePing::~StatePing() 16 | { 17 | 18 | } 19 | 20 | void StatePing::onEntry() 21 | { 22 | std::cout << "Enter Ping state:"; 23 | } 24 | 25 | void StatePing::onActive() 26 | { 27 | std::cout << " Ping" << std::flush; 28 | 29 | if(rand()%100<30) 30 | _agent->transitionToVolatileState(new StatePong(_model)); 31 | } 32 | 33 | void StatePing::onExit() 34 | { 35 | std::cout << " ... leaving" << std::endl << std::flush; 36 | } 37 | 38 | } /* namespace obvious */ 39 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StatePong.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "obcore/statemachine/Agent.h" 5 | #include "StatePong.h" 6 | #include "StatePing.h" 7 | 8 | namespace obvious 9 | { 10 | 11 | StatePong::StatePong(AgentModel* model) : StateBaseModel(model) 12 | { 13 | 14 | } 15 | 16 | StatePong::~StatePong() 17 | { 18 | 19 | } 20 | 21 | void StatePong::onEntry() 22 | { 23 | std::cout << "Enter Pong state:" << std::flush; 24 | } 25 | 26 | void StatePong::onActive() 27 | { 28 | std::cout << " Pong" << std::flush; 29 | 30 | if(rand()%100<30) 31 | _agent->transitionToVolatileState(new StatePing(_model)); 32 | } 33 | 34 | void StatePong::onExit() 35 | { 36 | std::cout << " ... leaving" << std::endl << std::flush; 37 | } 38 | 39 | } /* namespace obvious */ 40 | -------------------------------------------------------------------------------- /applications/kinect.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /installObviously: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | #set path of obviously to bashrc 4 | HERE=`dirname $(pwd)/obviously/` 5 | echo $HERE 6 | 7 | echo "export OBVIOUSLY_ROOT=$HERE" >> ~/.bashrc 8 | echo "source ~/.bashrc" 9 | 10 | #packages for obcore 11 | apt-get install gsl-bin 12 | apt-get install libgsl-dev 13 | apt-get install libudev-dev 14 | apt-get install libxml++2.6-dev 15 | apt-get install liblua5.1-0-dev 16 | apt-get install libeigen3-dev 17 | 18 | #packages for obdevice 19 | apt-get install ps-engine 20 | apt-get install libopenni-dev 21 | apt-get install libv4l-dev 22 | 23 | #packages for obgraphic 24 | apt-get install libvtk6-dev 25 | apt-get install libvtk6-qt-dev 26 | apt-get install freeglut3-dev 27 | 28 | #packages for obvision 29 | apt-get install libann-dev 30 | apt-get install libflann-dev 31 | 32 | -------------------------------------------------------------------------------- /applications/showCloud.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * showCloud.cpp 3 | * 4 | * Created on: 31.10.2012 5 | * Author: phil 6 | */ 7 | 8 | #include 9 | #include "obgraphic/Obvious3D.h" 10 | 11 | using namespace obvious; 12 | using namespace std; 13 | 14 | /** 15 | * This application shows a given cloud in the Obvious3D Viewer. 16 | * Author: Philipp Koch 17 | * Date: 31.10.2012 18 | */ 19 | 20 | int main(int argc, char **argv) 21 | { 22 | if(argc!=2) 23 | { 24 | cout<<"Usage: " << argv[0] << " <****.vtp> \n"; 25 | exit(1); 26 | } 27 | 28 | VtkCloud *vcloud; 29 | Obvious3D *viewer; 30 | 31 | vcloud = VtkCloud::load(argv[1], VTKCloud_AUTO); 32 | viewer=new Obvious3D("ShowCloud"); 33 | viewer->addCloud(vcloud); 34 | viewer->startRendering(); 35 | 36 | delete vcloud; 37 | delete viewer; 38 | 39 | return(0); 40 | 41 | } 42 | 43 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | obviously 2 | ========= 3 | 4 | Open Robotic Vision and Utilities Library 5 | You can install all neccessary packages by using the installObviously script 6 | Type "sudo ./installObviously" to the terminal. 7 | 8 | Initial Checkout 9 | ================ 10 | Use EGit extension to enable Eclipse integration (recommended) 11 | 12 | or use the command line 13 | 14 | git clone https://github.com/stefanmay/obviously.git 15 | 16 | There are project files for Eclipse. Please update the GIT index in order to ignore workstation specific changes: 17 | 18 | git update-index --assume-unchanged .project 19 | 20 | git update-index --assume-unchanged .cproject 21 | 22 | Configuration of GIT client 23 | 24 | git config --global user.name "Your Name Here" 25 | 26 | git config --global user.email "@ohm" 27 | 28 | git config --global credential.helper cache 29 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/IPreAssignmentFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef IPREASSIGNMENTFILTER_H 2 | #define IPREASSIGNMENTFILTER_H 3 | 4 | #include 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class IPreAssignmentFilter 11 | * @brief A pre-filter for point pair assignment 12 | * @author Stefan May 13 | */ 14 | class IPreAssignmentFilter 15 | { 16 | public: 17 | /** 18 | * Default constructor 19 | */ 20 | IPreAssignmentFilter(){_active=true;}; 21 | 22 | /** 23 | * Destructor 24 | */ 25 | virtual ~IPreAssignmentFilter(){}; 26 | 27 | virtual void filter(double** scene, unsigned int size, bool* mask) = 0; 28 | 29 | virtual void activate(){_active = true;}; 30 | 31 | virtual void deactivate(){_active = false;}; 32 | 33 | public: 34 | 35 | bool _active; 36 | 37 | }; 38 | 39 | } 40 | 41 | #endif /*IPREASSIGNMENTFILTER_H*/ 42 | -------------------------------------------------------------------------------- /obcore/math/Trajectory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Trajectory.h 3 | * 4 | * Created on: 30.01.2014 5 | * Author: user 6 | */ 7 | 8 | #ifndef TRAJECTORY_H_ 9 | #define TRAJECTORY_H_ 10 | 11 | #include 12 | #include "obcore/math/linalg/linalg.h" 13 | 14 | namespace obvious{ 15 | 16 | /** 17 | * @class Trajectory 18 | * @author Christian Pfitzner 19 | * @date 2014-01-30 20 | */ 21 | class Trajectory { 22 | public: 23 | /** 24 | * Standard Constructor 25 | */ 26 | Trajectory(); 27 | /** 28 | * Default destructor 29 | */ 30 | virtual ~Trajectory(); 31 | /** 32 | * Function to add single pose to trajectory 33 | * @param pose pose based on obvious::Matrix 4x4 34 | */ 35 | void setPose(const Matrix pose); 36 | 37 | std::vector getTrajectory(void) const; 38 | private: 39 | std::vector _trajectory; 40 | }; 41 | 42 | }; 43 | 44 | #endif /* TRAJECTORY_H_ */ 45 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StatePing.h: -------------------------------------------------------------------------------- 1 | #ifndef STATEPING_H_ 2 | #define STATEPING_H_ 3 | 4 | #include 5 | 6 | namespace obvious { 7 | 8 | /** 9 | * @class StatePing 10 | * @brief Example state, transition to pong state 11 | * @author Stefan May 12 | */ 13 | class StatePing : public StateBaseModel 14 | { 15 | public: 16 | 17 | /** 18 | * Constructor 19 | */ 20 | StatePing(AgentModel* model); 21 | 22 | /** 23 | * Destructor 24 | */ 25 | virtual ~StatePing(); 26 | 27 | /** 28 | * Called once when activated 29 | */ 30 | void onEntry(); 31 | 32 | /** 33 | * Process method (step-wise, never block this method) 34 | */ 35 | void onActive(); 36 | 37 | /** 38 | * Called once when left 39 | */ 40 | void onExit(); 41 | 42 | private: 43 | 44 | }; 45 | 46 | } /* namespace obvious */ 47 | 48 | #endif /* STATEPING_H_ */ 49 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StatePong.h: -------------------------------------------------------------------------------- 1 | #ifndef STATEPONG_H_ 2 | #define STATEPONG_H_ 3 | 4 | #include 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class StatePong 11 | * @brief Example state, transition to ping state 12 | * @author Stefan May 13 | */ 14 | class StatePong: public StateBaseModel 15 | { 16 | public: 17 | 18 | /** 19 | * Constructor 20 | */ 21 | StatePong(AgentModel* model); 22 | 23 | /** 24 | * Destructor 25 | */ 26 | virtual ~StatePong(); 27 | 28 | /** 29 | * Called once when activated 30 | */ 31 | void onEntry(); 32 | 33 | /** 34 | * Process method (step-wise, never block this method) 35 | */ 36 | void onActive(); 37 | 38 | /** 39 | * Called once when left 40 | */ 41 | void onExit(); 42 | 43 | private: 44 | 45 | }; 46 | 47 | } /* namespace obvious */ 48 | 49 | #endif /* STATEPONG_H_ */ 50 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.cpp: -------------------------------------------------------------------------------- 1 | #include "OutOfBoundsFilter2D.h" 2 | #include "obcore/base/System.h" 3 | #include 4 | 5 | namespace obvious 6 | { 7 | 8 | OutOfBoundsFilter2D::OutOfBoundsFilter2D(double xMin, double xMax, double yMin, double yMax) 9 | { 10 | _xMin = xMin; 11 | _xMax = xMax; 12 | _yMin = yMin; 13 | _yMax = yMax; 14 | _T = new Matrix(3, 3);; 15 | } 16 | 17 | OutOfBoundsFilter2D::~OutOfBoundsFilter2D() 18 | { 19 | delete _T; 20 | } 21 | 22 | void OutOfBoundsFilter2D::setPose(Matrix* T) 23 | { 24 | *_T = *T; 25 | } 26 | 27 | void OutOfBoundsFilter2D::filter(double** scene, unsigned int size, bool* mask) 28 | { 29 | Matrix S(size, 2, *scene); 30 | S.transform(*_T); 31 | 32 | for(unsigned int i=0; i_xMax || S(i,1)<_yMin || S(i,1)>_yMax) 35 | mask[i] = false; 36 | } 37 | } 38 | 39 | } 40 | 41 | -------------------------------------------------------------------------------- /obgraphic/Obvious.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of the Obviously library. 3 | * 4 | * Copyright(c) 2010-2012 Georg-Simon-Ohm University, Nuremberg. 5 | * http://www.ohm-university.eu 6 | * 7 | * This file may be licensed under the terms of of the 8 | * GNU General Public License Version 2 (the ``GPL''). 9 | * 10 | * Software distributed under the License is distributed 11 | * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either 12 | * express or implied. See the GPL for the specific language 13 | * governing rights and limitations. 14 | * 15 | * You should have received a copy of the GPL along with this 16 | * program. If not, go to http://www.gnu.org/licenses/gpl.html 17 | * or write to the Free Software Foundation, Inc., 18 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | * 20 | */ 21 | 22 | #ifndef OBVIOUS_H 23 | #define OBVIOUS_H 24 | 25 | typedef void (*fptrKeyboardCallback)(); 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/ReciprocalFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef RECIPROCALFILTER_H 2 | #define RECIPROCALFILTER_H 3 | 4 | #include "obvision/registration/icp/assign/filter/IPostAssignmentFilter.h" 5 | #include 6 | 7 | namespace obvious 8 | { 9 | 10 | /** 11 | * @class ReciprocalFilter 12 | * @brief A filter for point pair rejection 13 | * @author Stefan May 14 | */ 15 | class ReciprocalFilter : public IPostAssignmentFilter 16 | { 17 | public: 18 | /** 19 | * Default constructor 20 | */ 21 | ReciprocalFilter(); 22 | 23 | /** 24 | * Destructor 25 | */ 26 | ~ReciprocalFilter(); 27 | 28 | virtual void filter(double** model, double** scene, vector* pairs, vector* distancesSqr, vector* fpairs, vector* fdistancesSqr, vector* nonPairs); 29 | 30 | private: 31 | unsigned int _unOverlap; 32 | }; 33 | 34 | } 35 | 36 | #endif /*ReciprocalFilter_H*/ 37 | -------------------------------------------------------------------------------- /obcore/math/linalg/eigen/Vector.cpp: -------------------------------------------------------------------------------- 1 | #include "Vector.h" 2 | 3 | namespace obvious 4 | { 5 | 6 | Vector::Vector(unsigned int size, double* data) 7 | { 8 | _V.resize(size); 9 | if(data) 10 | { 11 | for(unsigned int i=0; i 5 | #include 6 | 7 | class PclCloudInterface 8 | { 9 | public: 10 | static void save(const double* coords, 11 | const unsigned char* rgb, 12 | const size_t width, 13 | const size_t height, 14 | const std::string& fileName); 15 | 16 | static void loadAllCloudsFromDirectory(const std::string& dir, 17 | std::vector& coords, 18 | std::vector& rgbs, 19 | std::vector& widths, 20 | std::vector& heights); 21 | 22 | private: 23 | static void getAllFileNamesFromDirectory(const std::string& dir, std::vector& fileNames); 24 | }; 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.h: -------------------------------------------------------------------------------- 1 | #ifndef OUTOFBOUNDSFILTER2D_H 2 | #define OUTOFBOUNDSFILTER2D_H 3 | 4 | #include "obvision/registration/icp/assign/filter/IPreAssignmentFilter.h" 5 | #include "obcore/math/linalg/linalg.h" 6 | 7 | namespace obvious 8 | { 9 | 10 | /** 11 | * @class OutOfBoundsFilter2D 12 | * @brief Bounding box filtering 13 | * @author Stefan May 14 | */ 15 | class OutOfBoundsFilter2D : public IPreAssignmentFilter 16 | { 17 | public: 18 | /** 19 | * Default constructor 20 | */ 21 | OutOfBoundsFilter2D(double xmin, double xmax, double ymin, double ymax); 22 | 23 | /** 24 | * Destructor 25 | */ 26 | ~OutOfBoundsFilter2D(); 27 | 28 | void setPose(Matrix* T); 29 | 30 | virtual void filter(double** scene, unsigned int size, bool* mask); 31 | 32 | private: 33 | double _xMin; 34 | double _xMax; 35 | double _yMin; 36 | double _yMax; 37 | Matrix* _T; 38 | }; 39 | 40 | } 41 | 42 | #endif /*OUTOFBOUNDSFILTER2D_H*/ 43 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/OcclusionFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef OCCLUSIONFILTER_H 2 | #define OCCLUSIONFILTER_H 3 | 4 | #include "obcore/base/CartesianCloud.h" 5 | #include "obvision/registration/icp/assign/filter/IPreAssignmentFilter.h" 6 | #include 7 | 8 | using namespace obvious; 9 | 10 | namespace obvious 11 | { 12 | 13 | /** 14 | * @class OcclusionFilter 15 | * @brief 16 | * @author Stefan May 17 | */ 18 | class OcclusionFilter : public IPreAssignmentFilter 19 | { 20 | public: 21 | /** 22 | * Default constructor 23 | */ 24 | OcclusionFilter(double* P, unsigned int width, unsigned int height); 25 | 26 | /** 27 | * Destructor 28 | */ 29 | ~OcclusionFilter(); 30 | 31 | void update(double* P); 32 | 33 | virtual void filter(double** scene, unsigned int size, bool* mask); 34 | 35 | private: 36 | double _P[12]; 37 | int** _map; 38 | double** _zbuffer; 39 | unsigned int _w; 40 | unsigned int _h; 41 | }; 42 | 43 | } 44 | 45 | #endif /*OCCLUSIONFILTER_H*/ 46 | -------------------------------------------------------------------------------- /applications/xtion.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /obvision/registration/amcl/AdaptiveMonteCarloMatching.h: -------------------------------------------------------------------------------- 1 | #ifndef ADAPTIVEMONTECARLOMATCHING_H_ 2 | #define ADAPTIVEMONTECARLOMATCHING_H_ 3 | 4 | #include 5 | #include "obcore/math/linalg/linalg.h" 6 | #include "obvision/registration/Trace.h" 7 | 8 | namespace obvious 9 | { 10 | 11 | /** 12 | * @class AdaptiveMonteCarloMatching 13 | * @brief Matching algorithm with AMCL alignment 14 | * @author Daniel Ammon, Tobias Fink and Stefan May 15 | **/ 16 | class AdaptiveMonteCarloMatching 17 | { 18 | public: 19 | /** 20 | * Constructor 21 | * @param 22 | */ 23 | AdaptiveMonteCarloMatching(); 24 | 25 | /** 26 | * Destructor 27 | */ 28 | virtual ~AdaptiveMonteCarloMatching(); 29 | 30 | /** 31 | * Matching method 32 | * @param ... map 33 | * @param S Matrix for scene points 34 | * @param maskS Mask for matrix S 35 | */ 36 | obvious::Matrix match(const obvious::Matrix* S, const bool* maskS); 37 | 38 | private: 39 | 40 | }; 41 | 42 | } 43 | 44 | #endif /* ADAPTIVEMONTECARLOMATCHING_H_ */ 45 | -------------------------------------------------------------------------------- /obvision/ransac/RansacPrimitives.h: -------------------------------------------------------------------------------- 1 | #ifndef RANSACPRIMITIVES_H_ 2 | #define RANSACPRIMITIVES_H_ 3 | 4 | #include "obcore/math/linalg/linalg.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class RansacPrimitives 11 | * @brief Ransac parameter estimator for geometric primitives 12 | * @author Stefan May 13 | * @date 22.10.2015 14 | */ 15 | class RansacPrimitives 16 | { 17 | public: 18 | /** 19 | * Constructor 20 | */ 21 | RansacPrimitives(unsigned int trials); 22 | 23 | /** 24 | * Destructor 25 | */ 26 | ~RansacPrimitives(); 27 | 28 | /** 29 | * Estimate parameters for circular primitives 30 | * @param D data matrix of size nx2, i.e. n tuples of xy-coordinates 31 | * @param params output vector of found circular parameters (centroid x,y and radius) 32 | * @param mask assignment mask (true=point belongs to circular structure) 33 | */ 34 | bool findCircle(obvious::Matrix* D, double params[3], bool* mask); 35 | 36 | private: 37 | 38 | unsigned int _trials; 39 | }; 40 | 41 | }// namespace 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.h: -------------------------------------------------------------------------------- 1 | #ifndef OUTOFBOUNDSFILTER3D_H 2 | #define OUTOFBOUNDSFILTER3D_H 3 | 4 | #include "obvision/registration/icp/assign/filter/IPreAssignmentFilter.h" 5 | #include "obcore/math/linalg/linalg.h" 6 | 7 | namespace obvious 8 | { 9 | 10 | /** 11 | * @class OutOfBoundsFilter3D 12 | * @brief Bounding box filtering 13 | * @author Stefan May 14 | */ 15 | class OutOfBoundsFilter3D : public IPreAssignmentFilter 16 | { 17 | public: 18 | /** 19 | * Default constructor 20 | */ 21 | OutOfBoundsFilter3D(double xmin, double xmax, double ymin, double ymax, double zmin, double zmax); 22 | 23 | /** 24 | * Destructor 25 | */ 26 | ~OutOfBoundsFilter3D(); 27 | 28 | void setPose(Matrix* T); 29 | 30 | virtual void filter(double** scene, unsigned int size, bool* mask); 31 | 32 | private: 33 | double _xMin; 34 | double _xMax; 35 | double _yMin; 36 | double _yMax; 37 | double _zMin; 38 | double _zMax; 39 | Matrix* _T; 40 | }; 41 | 42 | } 43 | 44 | #endif /*OUTOFBOUNDSFILTER3D_H*/ 45 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/TrimmedFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef TRIMMEDFILTER_H 2 | #define TRIMMEDFILTER_H 3 | 4 | #include "obvision/registration/icp/assign/filter/IPostAssignmentFilter.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class TrimmedFilter 11 | * @brief A filter for point pair rejection 12 | * @author Stefan May 13 | */ 14 | class TrimmedFilter : public IPostAssignmentFilter 15 | { 16 | public: 17 | /** 18 | * Default constructor 19 | */ 20 | TrimmedFilter(unsigned int unOverlap); 21 | 22 | /** 23 | * Destructor 24 | */ 25 | ~TrimmedFilter(); 26 | 27 | virtual void filter(double** model, 28 | double** scene, 29 | vector* pairs, 30 | vector* distancesSqr, 31 | vector* fpairs, 32 | vector* fdistancesSqr, 33 | vector* nonPairs); 34 | 35 | private: 36 | unsigned int _unOverlap; 37 | }; 38 | 39 | } 40 | 41 | #endif /*TRIMMEDFILTER_H*/ 42 | -------------------------------------------------------------------------------- /obcore/filter/EuclideanFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * EuclideanFilter.h 3 | * 4 | * Created on: 29.11.2012 5 | * Author: phil 6 | */ 7 | 8 | #ifndef EUCLIDEANFilter_H_ 9 | #define EUCLIDEANFilter_H_ 10 | 11 | #include "obcore/filter/FilterDistance.h" 12 | 13 | /** 14 | * @namespace obvious 15 | */ 16 | namespace obvious 17 | { 18 | /** 19 | * @class EuclideanFilter 20 | * @brief Filter to push values from the input into the output buffer. 21 | * Values which amount is bigger than a threshold, won't be copied. 22 | */ 23 | class EuclideanFilter : public FilterDistance 24 | { 25 | public: 26 | /** 27 | * Default constructor 28 | */ 29 | EuclideanFilter(void) 30 | : FilterDistance() { } 31 | /** 32 | * virtual destructor 33 | */ 34 | virtual ~EuclideanFilter(void) { } 35 | /** 36 | * Function to start the Filter 37 | */ 38 | FILRETVAL applyFilter(void); 39 | /** 40 | * Function to set centroid for filtering 41 | * @param center center point 42 | */ 43 | void setCentroid(const Point3D& center); 44 | }; 45 | 46 | }; // namespace 47 | 48 | #endif /* EUCLIDEANFilter_H_ */ 49 | -------------------------------------------------------------------------------- /applications/lua_call_function.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "obcore/scripting/LuaScriptManager.h" 4 | 5 | using namespace std; 6 | using namespace obvious; 7 | 8 | /** 9 | * Example showing usage of LuaScriptManager: Calling Lua functions 10 | * @author Stefan May 11 | * @date 16.6.2015 12 | */ 13 | int main(int argc, char* argv[]) 14 | { 15 | if(argc!=2) 16 | { 17 | cout << "usage: " << argv[0] << " .../lua/functionWithCallback.lua" << endl; 18 | return -1; 19 | } 20 | 21 | LuaScriptManager manager(argv[1]); 22 | 23 | while(1) 24 | { 25 | double x = 3.0; 26 | double y = 7.0; 27 | double z; 28 | 29 | // define input as: double, double 30 | // define output (>) as: double 31 | manager.callFunction("aLuaFunction", "dd>d", x, y, &z); 32 | cout << "Called with parameters x=" << x << ", y=" << y << endl; 33 | cout << " returned value z=" << z << endl; 34 | 35 | // slow down loop 36 | usleep(500000); 37 | 38 | // reloading allows to modify Lua function during runtime 39 | manager.reload(); 40 | } 41 | 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /applications/statemachine_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "obcore/statemachine/AgentModel.h" 4 | #include "obcore/statemachine/states/StatePing.h" 5 | 6 | using namespace obvious; 7 | 8 | /** 9 | * Example showing usage of obviously state machine 10 | * @author Stefan May 11 | * @date 17.6.2015 12 | */ 13 | int main(int argc, char* argv[]) 14 | { 15 | // The state machine might use a user-defined model. For that purpose use your own model class instances and pass them to your states. 16 | AgentModel model; 17 | 18 | // Instantiate generic agent. It processes activated states. You can use C++ states as well as Lua states (see lua_statemachine example). 19 | Agent agent; 20 | 21 | // Pass first state to agent. 22 | agent.transitionToVolatileState(new StatePing(&model)); 23 | 24 | while(true) 25 | { 26 | // Process current state and make necessary transitions. 27 | // If no state has been passed to the agent, awake returns without effect. 28 | agent.awake(); 29 | 30 | // Slow down loop to make monitor console output readable 31 | usleep(100000); 32 | } 33 | 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /obvision/registration/icp/icp_def.h: -------------------------------------------------------------------------------- 1 | #include "obvision/registration/icp/assign/AnnPairAssignment.h" 2 | #include "obvision/registration/icp/assign/FlannPairAssignment.h" 3 | #include "obvision/registration/icp/assign/ProjectivePairAssignment.h" 4 | #include "obvision/registration/icp/assign/filter/ProjectionFilter.h" 5 | #include "obvision/registration/icp/assign/filter/OcclusionFilter.h" 6 | #include "obvision/registration/icp/assign/filter/TrimmedFilter.h" 7 | #include "obvision/registration/icp/assign/filter/ReciprocalFilter.h" 8 | #include "obvision/registration/icp/assign/filter/DistanceFilter.h" 9 | #include "obvision/registration/icp/assign/filter/OutOfBoundsFilter2D.h" 10 | #include "obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.h" 11 | #include "obvision/registration/icp/assign/filter/RobotFootprintFilter.h" 12 | #include "obvision/registration/icp/PointToPointEstimator3D.h" 13 | #include "obvision/registration/icp/PointToPlaneEstimator3D.h" 14 | #include "obvision/registration/icp/ClosedFormEstimator2D.h" 15 | #include "obvision/registration/icp/PointToLineEstimator2D.h" 16 | #include "obvision/registration/icp/Icp.h" 17 | -------------------------------------------------------------------------------- /applications/kinect_ir.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /obcore/math/linalg/gsl/Vector.cpp: -------------------------------------------------------------------------------- 1 | #include "Vector.h" 2 | 3 | namespace obvious 4 | { 5 | Vector::Vector(unsigned int size, double* data) 6 | { 7 | _V = gsl_vector_alloc(size); 8 | if(data) 9 | setData(data); 10 | } 11 | 12 | Vector::Vector(const Vector &V) 13 | { 14 | _V = gsl_vector_alloc(V._V->size); 15 | gsl_vector_memcpy(_V, V._V); 16 | } 17 | 18 | Vector::~Vector() 19 | { 20 | gsl_vector_free(_V); 21 | } 22 | 23 | Vector& Vector::operator = (const Vector &V) 24 | { 25 | gsl_vector_memcpy(_V, V._V); 26 | return *this; 27 | } 28 | 29 | double& Vector::operator () (unsigned int i) 30 | { 31 | return *gsl_vector_ptr(_V, i); 32 | } 33 | 34 | unsigned int Vector::getSize() 35 | { 36 | return _V->size; 37 | } 38 | 39 | void Vector::setData(const double* array) 40 | { 41 | gsl_vector_const_view varray = gsl_vector_const_view_array(array, _V->size); 42 | gsl_vector_memcpy(_V, &varray.vector); 43 | } 44 | 45 | void Vector::setZero() 46 | { 47 | gsl_vector_set_zero(_V); 48 | } 49 | 50 | void Vector::print() 51 | { 52 | for(unsigned int i=0; i<_V->size; i++) 53 | cout << gsl_vector_get(_V, i); 54 | cout << endl; 55 | } 56 | 57 | } 58 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/DistanceFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef DISTANCEFILTER_H 2 | #define DISTANCEFILTER_H 3 | 4 | #include "obvision/registration/icp/assign/filter/IPostAssignmentFilter.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class DistanceFilter 11 | * @brief A filter for point pair rejection 12 | * @author Stefan May 13 | */ 14 | class DistanceFilter : public IPostAssignmentFilter 15 | { 16 | public: 17 | /** 18 | * Default constructor 19 | */ 20 | DistanceFilter(double maxdist, double mindist, unsigned int iterations); 21 | 22 | /** 23 | * Destructor 24 | */ 25 | ~DistanceFilter(); 26 | 27 | virtual void filter(double** model, double** scene, 28 | vector* pairs, 29 | vector* distancesSqr, 30 | vector* fpairs, 31 | vector* fdistancesSqr, 32 | vector* nonPairs); 33 | 34 | virtual void reset(); 35 | private: 36 | double _maxDistSqr; 37 | double _minDistSqr; 38 | double _distSqr; 39 | double _multiplier; 40 | }; 41 | 42 | } 43 | 44 | #endif /*DISTANCEFILTER_H*/ 45 | -------------------------------------------------------------------------------- /obcore/filter/EuclideanFilterVecD.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * EuclideanFilterVecD.cpp 3 | * 4 | * Created on: 30.11.2012 5 | * Author: phil 6 | */ 7 | 8 | #include "obcore/filter/EuclideanFilterVecD.h" 9 | #include "obcore/math/mathbase.h" 10 | #include 11 | #include 12 | 13 | using namespace obvious; 14 | 15 | double absVector(vector::iterator start); 16 | 17 | FILRETVAL EuclideanFilterVecD::applyFilter(void) 18 | { 19 | vector::iterator inIter=_input->begin(); 20 | double abs=0; 21 | unsigned int outCtr=0; 22 | 23 | for(unsigned int i=0;i<((unsigned int)_input->size())/3;i++) 24 | { 25 | abs=absVector(inIter); 26 | // abs=euklideanDistanceVecD(&inIter,NULL); 27 | if(abs<_threshold) 28 | { 29 | for(unsigned int j=0;j<3;j++) 30 | { 31 | _output->push_back(*inIter++); 32 | outCtr++; 33 | } 34 | } 35 | else 36 | inIter+=3; 37 | } 38 | 39 | return(FILTER_OK); 40 | } 41 | 42 | double absVector(vector::iterator start) 43 | { 44 | double abs=0; 45 | vector::iterator iter=start; 46 | for(unsigned int i=0;i<3;i++) 47 | { 48 | abs+=*iter*(*iter); 49 | iter++; 50 | } 51 | return(std::sqrt(abs)); 52 | } 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /test/obcore/base/pointcloud.cpp: -------------------------------------------------------------------------------- 1 | #include "obcore/base/PointCloud.h" 2 | #include "obcore/base/Point.h" 3 | #include "obdevice/CloudFactory.h" 4 | #include "obgraphic/CloudWidget.h" 5 | #include "obcore/base/Timer.h" 6 | #include "obcore/math/linalg/gsl/Matrix.h" 7 | 8 | #include 9 | 10 | int main(int argc, char** argv) 11 | { 12 | QApplication app(argc, argv); 13 | obvious::CloudWidget viewA; 14 | obvious::CloudWidget viewB; 15 | viewA.show(); 16 | viewB.show(); 17 | 18 | obvious::PointCloud cloud; 19 | // obvious::CloudFactory::generateRandomCloud(cloud, 1000); 20 | obvious::CloudFactory::loadCloud(cloud, "/home/knueppl/git/libra3d/build/12:55:10.386.pcd"); 21 | viewA.setCloud(cloud); 22 | 23 | obvious::Timer timer; 24 | timer.start(); 25 | 26 | cloud.rotate(90.0f * M_PI / 180.0f, 0.0f, 0.0f); 27 | 28 | std::cout << "time = " << timer.elapsed() << std::endl; 29 | viewB.setCloud(cloud); 30 | 31 | obvious::Matrix mat(cloud.size(), 3); 32 | obvious::Matrix rot(3, 3); 33 | timer.reset(); 34 | mat *= rot; 35 | std::cout << "time = " << timer.elapsed() << std::endl; 36 | 37 | return app.exec(); 38 | } 39 | -------------------------------------------------------------------------------- /obvision/normals/NormalsEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef NORMALSESTIMATOR_H_ 2 | #define NORMALSESTIMATOR_H_ 3 | 4 | #include 5 | using namespace std; 6 | 7 | #include "obcore/base/CartesianCloud.h" 8 | #include "obcore/base/System.h" 9 | 10 | #include "obcore/math/linalg/linalg.h" 11 | 12 | using namespace obvious; 13 | 14 | namespace obvious 15 | { 16 | 17 | /** 18 | * @class NormalsEstimator 19 | * @brief Estimation of normals of a point cloud 20 | * @author Stefan May 21 | **/ 22 | class NormalsEstimator 23 | { 24 | public: 25 | /** 26 | * Standard constructor 27 | */ 28 | NormalsEstimator(); 29 | 30 | /** 31 | * Destructor 32 | */ 33 | ~NormalsEstimator(); 34 | 35 | /** 36 | * 37 | */ 38 | void estimateNormals3DGrid(unsigned int cols, unsigned int rows, double* coords, bool* mask, double* normals); 39 | 40 | /** 41 | * 42 | */ 43 | void estimateNormalsReverseMapping(Matrix* coords, Matrix* P, int w, int h, Matrix* normals); 44 | 45 | /** 46 | * 47 | */ 48 | void estimateNormalsFLANN(Matrix* coords, Matrix* normals); 49 | void estimateNormalsANN(Matrix* coords, Matrix* normals); 50 | 51 | private: 52 | 53 | }; 54 | 55 | } 56 | 57 | #endif /*NORMALSESTIMATOR_H_*/ 58 | -------------------------------------------------------------------------------- /applications/ransac_circle.cpp: -------------------------------------------------------------------------------- 1 | #include "obvision/ransac/RansacPrimitives.h" 2 | 3 | #include 4 | #include 5 | 6 | using namespace obvious; 7 | using namespace std; 8 | 9 | int main(int argc, char* argv[]) 10 | { 11 | const unsigned int n=1000; 12 | Matrix D(n, 2); 13 | bool mask[n]; 14 | 15 | // Generate data 16 | double x0 = 4.0; 17 | double y0 = 5.0; 18 | double r = 6.0; 19 | double s = 1.0; 20 | for(unsigned int i=0; i 7 | 8 | using namespace obvious; 9 | 10 | namespace obvious 11 | { 12 | 13 | /** 14 | * @class ProjectionFilter 15 | * @brief 16 | * @author Stefan May 17 | */ 18 | class ProjectionFilter : public IPreAssignmentFilter 19 | { 20 | public: 21 | /** 22 | * Default constructor 23 | */ 24 | ProjectionFilter(double* P, unsigned int width, unsigned int height); 25 | 26 | /** 27 | * Destructor 28 | */ 29 | ~ProjectionFilter(); 30 | 31 | void setModel(CartesianCloud3D* cloud); 32 | void update(CartesianCloud3D* cloud, double* P); 33 | void update(double* P, unsigned char* mask, double* zbuffer); 34 | void enableFaultRemovement(); 35 | void disableFaultRemovement(); 36 | 37 | virtual void filter(double** scene, unsigned int size, bool* mask); 38 | 39 | private: 40 | double _P[12]; 41 | unsigned char** _mask; 42 | double** _zbuffer; 43 | unsigned int _w; 44 | unsigned int _h; 45 | double _zFar; 46 | bool _faultRemovement; 47 | }; 48 | 49 | } 50 | 51 | #endif /*PROJECTIONFILTER_H*/ 52 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/IPostAssignmentFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef IPOSTASSIGNMENTFILTER_H 2 | #define IPOSTASSIGNMENTFILTER_H 3 | 4 | #include "obvision/registration/icp/assign/assignbase.h" 5 | 6 | #include 7 | 8 | using namespace std; 9 | 10 | namespace obvious 11 | { 12 | 13 | /** 14 | * @class IPostAssignmentFilter 15 | * @brief A filter for point pair rejection 16 | * @author Stefan May 17 | */ 18 | class IPostAssignmentFilter 19 | { 20 | public: 21 | /** 22 | * Default constructor 23 | */ 24 | IPostAssignmentFilter(){_active = true;}; 25 | 26 | /** 27 | * Destructor 28 | */ 29 | virtual ~IPostAssignmentFilter(){}; 30 | 31 | virtual void filter(double** model, double** scene, 32 | vector* pairs, 33 | vector* distancesSqr, 34 | vector* fpairs, 35 | vector* fdistancesSqr, 36 | vector* nonPairs) = 0; 37 | 38 | virtual void reset(){}; 39 | 40 | virtual void activate(){_active = true;}; 41 | 42 | virtual void deactivate(){_active = false;}; 43 | 44 | public: 45 | 46 | bool _active; 47 | }; 48 | 49 | } 50 | 51 | #endif /*IPOSTASSIGNMENTFILTER_H*/ 52 | -------------------------------------------------------------------------------- /obcore/math/IntegratorSimpson.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * IntegratorSimpson.cpp 3 | * 4 | * Created on: 04.09.2013 5 | * Author: chris 6 | */ 7 | 8 | #include "IntegratorSimpson.h" 9 | #include "stdio.h" 10 | #include 11 | #include 12 | 13 | using namespace obvious; 14 | 15 | IntegratorSimpson::IntegratorSimpson(void) 16 | { 17 | _upperLimit = NAN; 18 | _lowerLimit = -NAN; 19 | _T_n = 1.0; 20 | _e_n = _e_n_1 = _e_n_2 = 0.0; 21 | _y_n = _y_n_1 = _y_n_2 = 0.0; 22 | } 23 | 24 | IntegratorSimpson::~IntegratorSimpson(void) 25 | { 26 | 27 | } 28 | 29 | void IntegratorSimpson::setRestTime(const double& T_n) 30 | { 31 | _T_n = T_n; 32 | } 33 | 34 | void IntegratorSimpson::setAntiWindup(const double& upperLimit, const double& lowerLimit) 35 | { 36 | _upperLimit = upperLimit; 37 | _lowerLimit = lowerLimit; 38 | } 39 | 40 | const double& IntegratorSimpson::integrate(const double& error, const double& deltaT) 41 | { 42 | _e_n_2 = _e_n_1; 43 | _e_n_1 = _e_n; 44 | _e_n = error; 45 | 46 | _y_n_2 = _y_n_1; 47 | _y_n_1 = _y_n; 48 | _y_n = _y_n_1 + (1.0/_T_n)*(_e_n/6.0 + 4.0*_e_n_1/6.0 + _e_n_2/6.0) * deltaT; 49 | 50 | return(_y_n); 51 | } 52 | 53 | void IntegratorSimpson::resetIntegrator(const double& setValue) 54 | { 55 | _y_n = setValue; 56 | } 57 | 58 | -------------------------------------------------------------------------------- /obvision/registration/icp/IcpMultiInitIterator.h: -------------------------------------------------------------------------------- 1 | #ifndef ICPMULTIINITITERATOR_H_ 2 | #define ICPMULTIINITITERATOR_H_ 3 | 4 | #include 5 | using namespace std; 6 | 7 | #include "obvision/registration/icp/Icp.h" 8 | 9 | using namespace obvious; 10 | 11 | namespace obvious 12 | { 13 | 14 | /** 15 | * @class IcpMultiInitIterator 16 | * @brief Iterates through vector of initialization matrices and tests for the best matching result. 17 | * This class is to be used for registration under fast motion 18 | * @author Stefan May 19 | **/ 20 | class IcpMultiInitIterator 21 | { 22 | public: 23 | /** 24 | * Standard constructor 25 | * @param assigner pair assignment strategy for point clouds 26 | * @param estimator transformation estimator 27 | * @param pDistanceThreshold thresholding strategy 28 | */ 29 | IcpMultiInitIterator(vector Tinit); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~IcpMultiInitIterator(); 35 | 36 | /** 37 | * Start iteration for all initialization matrices. Take best result. 38 | * @param icp Instance of iterative closest point class 39 | */ 40 | Matrix iterate(Icp* icp); 41 | 42 | private: 43 | 44 | vector _Tinit; 45 | 46 | Matrix* _Tlast; 47 | }; 48 | 49 | } 50 | 51 | #endif /*ICPMULTIINITITERATOR_H_*/ 52 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/OutOfBoundsFilter3D.cpp: -------------------------------------------------------------------------------- 1 | #include "OutOfBoundsFilter3D.h" 2 | #include "obcore/base/System.h" 3 | #include "obcore/base/Logger.h" 4 | #include 5 | 6 | namespace obvious 7 | { 8 | 9 | OutOfBoundsFilter3D::OutOfBoundsFilter3D(double xMin, double xMax, double yMin, double yMax, double zMin, double zMax) 10 | { 11 | _xMin = xMin; 12 | _xMax = xMax; 13 | _yMin = yMin; 14 | _yMax = yMax; 15 | _zMin = zMin; 16 | _zMax = zMax; 17 | _T = new Matrix(4, 4); 18 | } 19 | 20 | OutOfBoundsFilter3D::~OutOfBoundsFilter3D() 21 | { 22 | delete _T; 23 | } 24 | 25 | void OutOfBoundsFilter3D::setPose(Matrix* T) 26 | { 27 | *_T = *T; 28 | } 29 | 30 | void OutOfBoundsFilter3D::filter(double** scene, unsigned int size, bool* mask) 31 | { 32 | Matrix S(size, 3, *scene); 33 | 34 | // Transform measurements back to world coordinate system 35 | S.transform(*_T); 36 | 37 | // int cnt = 0; 38 | for(unsigned int i=0; i_xMax || S(i,1)<_yMin || S(i,1)>_yMax || S(i,2)<_zMin || S(i,2)>_zMax) 42 | { 43 | mask[i] = false; 44 | } 45 | // else 46 | // cnt++; 47 | } 48 | 49 | // LOGMSG(DBG_DEBUG, "Bounding Box left: " << cnt << " points"); 50 | 51 | } 52 | 53 | } 54 | 55 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StateBaseModel.h: -------------------------------------------------------------------------------- 1 | #ifndef STATEBASEAGENT_H_ 2 | #define STATEBASEAGENT_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "obcore/statemachine/states/StateBase.h" 8 | #include "obcore/statemachine/AgentModel.h" 9 | /** 10 | * @namespace obvious 11 | */ 12 | namespace obvious 13 | { 14 | 15 | /** 16 | * @class StateBaseModel 17 | * @brief This is an example of extending StateBase to allow access to an agent model. 18 | * Implement your own StateBaseModel class to provide a specific model class instance to your states. 19 | * @author Stefan May 20 | * @date 19.6.2015 21 | */ 22 | class StateBaseModel : public StateBase 23 | { 24 | 25 | public: 26 | 27 | /** 28 | * Constructor 29 | */ 30 | StateBaseModel(AgentModel* model) { _model = model; }; 31 | 32 | /** 33 | * Default destructor 34 | */ 35 | virtual ~StateBaseModel(){}; 36 | 37 | /** 38 | * Called once when activated 39 | */ 40 | virtual void onEntry() { }; 41 | 42 | /** 43 | * Called while active 44 | */ 45 | virtual void onActive() = 0; 46 | 47 | /** 48 | * Called once when left 49 | */ 50 | virtual void onExit() { }; 51 | 52 | protected: 53 | 54 | AgentModel* _model; 55 | 56 | }; 57 | 58 | } /* namespace obvious */ 59 | 60 | #endif /* STATEBASEMODEL_H_ */ 61 | -------------------------------------------------------------------------------- /obdevice/ParentDevice3D.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ParentDevice3D.cpp 3 | * @author Christian Pfitzner 4 | * @date 07.01.2013 5 | * 6 | * 7 | */ 8 | 9 | #include "obdevice/ParentDevice3D.h" 10 | 11 | using namespace obvious; 12 | 13 | ParentDevice3D::ParentDevice3D(unsigned int cols, unsigned int rows) 14 | : _coords(NULL), _mask(NULL), _z(NULL), 15 | _rows(rows), _cols(cols), 16 | _frameRate(0.0f), 17 | _record(false) 18 | { 19 | _cols = cols; 20 | _rows = rows; 21 | _coords = new double[_rows*_cols*3]; 22 | _mask = new bool[_rows*_cols]; 23 | _z = new double[_rows*_cols]; 24 | _rgb = new unsigned char[_rows*_cols*3]; 25 | 26 | _timer.start(); 27 | } 28 | 29 | 30 | ParentDevice3D::~ParentDevice3D() 31 | { 32 | delete [] _coords; 33 | delete [] _z; 34 | delete [] _mask; 35 | delete [] _rgb; 36 | } 37 | 38 | /* 39 | * Function to estimate frame rate 40 | */ 41 | void ParentDevice3D::estimateFrameRate(void) 42 | { 43 | _frameRate = 1.0f / _timer.reset(); 44 | } 45 | 46 | void ParentDevice3D::startRecording(char* filename) 47 | { 48 | _recfile.open(filename, std::ios::out); 49 | _recfile << _cols << " " << _rows << std::endl; 50 | _record = true; 51 | } 52 | 53 | void ParentDevice3D::stopRecording() 54 | { 55 | _recfile.close(); 56 | _record = false; 57 | } 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /obgraphic/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | ############################ 4 | # General project settings # 5 | ############################ 6 | 7 | PROJECT(OBGRAPHIC) 8 | SET(OBCORE_VERSION_MAJOR 0) 9 | SET(OBCORE_VERSION_MINOR 1) 10 | 11 | FIND_PACKAGE(VTK REQUIRED) 12 | FIND_PACKAGE(Eigen REQUIRED) 13 | FIND_PACKAGE(Qt4) 14 | 15 | INCLUDE(${VTK_USE_FILE}) 16 | 17 | SET(SOURCE 18 | Obvious2D.cpp 19 | Obvious2DMap.cpp 20 | Obvious3D.cpp 21 | VtkCloud.cpp 22 | ) 23 | 24 | # Qt part 25 | IF(QT_FOUND) 26 | INCLUDE(${QT_USE_FILE}) 27 | ADD_DEFINITIONS(${QT_DEFINITIONS}) 28 | 29 | QT4_WRAP_CPP(MOC 30 | CloudWidget.h 31 | ) 32 | 33 | SET(SOURCE 34 | ${SOURCE} 35 | CloudWidget.cpp 36 | ${MOC} 37 | ) 38 | ENDIF() 39 | 40 | LINK_LIBRARIES(${VTK_LIBRARIES}) 41 | 42 | INCLUDE_DIRECTORIES(.. 43 | ${EIGEN_INCLUDE_DIRS} 44 | ) 45 | 46 | add_library(obgraphic STATIC 47 | ${SOURCE} 48 | ) 49 | 50 | #################### 51 | ##### Packaging #### 52 | #################### 53 | IF(CMAKE_BUILD_TYPE MATCHES Release) 54 | INSTALL(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" DESTINATION ${OBVIOUSLY_INC_DIR} FILES_MATCHING PATTERN "*.h") 55 | INSTALL(TARGETS obgraphic ARCHIVE DESTINATION ${OBVIOUSLY_LIB_DIR}) 56 | ENDIF() -------------------------------------------------------------------------------- /obvision/registration/ransacMatching/RandomMatching.h: -------------------------------------------------------------------------------- 1 | #ifndef _RANDOMMATCHING_H_ 2 | #define _RANDOMMATCHING_H_ 3 | 4 | #include 5 | #include "obcore/math/linalg/linalg.h" 6 | #include "obvision/registration/Trace.h" 7 | 8 | namespace obvious 9 | { 10 | 11 | class RandomMatching 12 | { 13 | public: 14 | RandomMatching(unsigned int sizeControlSet); 15 | 16 | virtual ~RandomMatching(); 17 | 18 | void activateTrace(); 19 | 20 | void deactivateTrace(); 21 | 22 | void serializeTrace(const char* folder); 23 | 24 | protected: 25 | 26 | // extract valid indices from matrix giving a validity mask 27 | std::vector extractSamples(obvious::Matrix* M, const bool* mask, unsigned int searchRange); 28 | 29 | // pick control set for RANSAC in-/outlier detection 30 | obvious::Matrix* pickControlSet(obvious::Matrix* M, std::vector idxValid, std::vector &idxControl); 31 | 32 | void calcNormals(Matrix* M, Matrix* N, const bool* maskIn, bool* maskOut, int searchRadius); 33 | 34 | void calcPhi(Matrix* N, const bool* mask, double* phi); 35 | 36 | void subsampleMask(bool* mask, unsigned int size, double probability); 37 | 38 | unsigned int _sizeControlSet; 39 | 40 | // Trace module 41 | Trace* _trace; 42 | 43 | }; 44 | 45 | } /* namespace obvious */ 46 | 47 | #endif /* _RANDOMMATCHING_H_ */ 48 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StateBase.h: -------------------------------------------------------------------------------- 1 | #ifndef STATEBASE_H_ 2 | #define STATEBASE_H_ 3 | 4 | #include 5 | #include 6 | 7 | /** 8 | * @namespace obvious 9 | */ 10 | namespace obvious 11 | { 12 | 13 | class Agent; 14 | 15 | /** 16 | * @class StateBase 17 | * @author Stefan May 18 | * @date 23.10.2014 19 | */ 20 | class StateBase 21 | { 22 | 23 | public: 24 | 25 | /** 26 | * Constructor 27 | */ 28 | StateBase(); 29 | 30 | /** 31 | * Default destructor 32 | */ 33 | virtual ~StateBase(); 34 | 35 | /** 36 | * Set agent of state. This is called by the agent at state transitions. 37 | * @param agent Agent instance 38 | */ 39 | void setAgent(Agent* agent); 40 | 41 | /** 42 | * Get agent of state. Default is NULL, when no agent is assigned. 43 | * @return pointer to Agent instance. 44 | */ 45 | Agent* getAgent(); 46 | 47 | /** 48 | * Called once when registered at Agent 49 | */ 50 | virtual void onSetup() {}; 51 | 52 | /** 53 | * Called once when activated 54 | */ 55 | virtual void onEntry() {}; 56 | 57 | /** 58 | * Called while active 59 | */ 60 | virtual void onActive() = 0; 61 | 62 | /** 63 | * Called once when left 64 | */ 65 | virtual void onExit() {}; 66 | 67 | protected: 68 | 69 | Agent* _agent; 70 | 71 | }; 72 | 73 | } /* namespace obvious */ 74 | 75 | #endif /* STATEBASE_H_ */ 76 | -------------------------------------------------------------------------------- /obcore/filter/CartesianFilter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file CartesianFilter.h 3 | * @author Christian Pfitzner 4 | * @date 03.12.2012 5 | * 6 | * 7 | */ 8 | 9 | #ifndef CARTESIANFILTER_H_ 10 | #define CARTESIANFILTER_H_ 11 | 12 | #include "obcore/filter/FilterDistance.h" 13 | 14 | /** 15 | * @namespace obvious 16 | */ 17 | namespace obvious 18 | { 19 | class FilterDistance; 20 | /** 21 | * @class CartesianFilter 22 | * Class to filter points out of a given dataset of 3d points in a double array 23 | */ 24 | class CartesianFilter : public FilterDistance 25 | { 26 | public: 27 | /** 28 | * Default constructor with initialization 29 | */ 30 | CartesianFilter(void) 31 | : FilterDistance(), 32 | _axis(x){ } 33 | ~CartesianFilter(void) { } 34 | /** 35 | * Function to start filtering 36 | * 37 | * @note This function must be called after setting input and output of 38 | * base class. 39 | */ 40 | FILRETVAL applyFilter(void); 41 | /** 42 | * Function to set specified axis for filtering 43 | * @param a defined axis 44 | */ 45 | void setAxis(const Axis& a); 46 | /** 47 | * Function to set centroid for filtering 48 | * @param center center point 49 | */ 50 | void setCentroid(const Point3D& center); 51 | private: 52 | Axis _axis; ///< axis to filter 53 | }; 54 | 55 | }; //namespace 56 | 57 | #endif /* CARTESIANFILTER_H_ */ 58 | -------------------------------------------------------------------------------- /License.txt: -------------------------------------------------------------------------------- 1 | Files within this source tree are part of the Obviously library 2 | written between 2010-2012 at the Georg-Simon-Ohm University, Nuremberg, 3 | http://www.ohm-university.eu 4 | 5 | The library is build upon several open-source libraries, namely 6 | - GNU Scientific Library: http://www.gnu.org/software/gsl/ 7 | - OpenNI: openni.org 8 | See related license informations. 9 | 10 | Some publicly available algorithms were included for convenience 11 | and are cited directly in the source code. 12 | If you aim at reusing the Obviously project, you need to clarify 13 | whether you can agree to related licenses. 14 | 15 | Remaining code is dedicated to research purposes and, if separable, 16 | under the following terms of license: 17 | 18 | Copyright(c) 2010-2012 Georg-Simon-Ohm University, Nuremberg, 19 | http://www.ohm-university.eu 20 | 21 | All files may be licensed under the terms of the 22 | GNU General Public License Version 2 (the ``GPL''). 23 | 24 | Software distributed under the License is distributed 25 | on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either 26 | express or implied. See the GPL for the specific language 27 | governing rights and limitations. 28 | 29 | You should have received a copy of the GPL along with this 30 | program. If not, go to http://www.gnu.org/licenses/gpl.html 31 | or write to the Free Software Foundation, Inc., 32 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 33 | -------------------------------------------------------------------------------- /applications/lua_callback_c.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "obcore/scripting/LuaScriptManager.h" 6 | 7 | using namespace std; 8 | using namespace obvious; 9 | 10 | /** 11 | * Callback function (called from Lua script) 12 | */ 13 | static int l_sin (lua_State *L) 14 | { 15 | double d = luaL_checknumber(L, 1); 16 | lua_pushnumber(L, sin(d)); 17 | return 1; /* number of results */ 18 | } 19 | 20 | /** 21 | * Example showing usage of LuaScriptManager: Calling Lua functions / Callback to C 22 | * @author Stefan May 23 | * @date 16.6.2015 24 | */ 25 | int main(int argc, char* argv[]) 26 | { 27 | if(argc!=2) 28 | { 29 | cout << "usage: " << argv[0] << " .../lua/function.lua" << endl; 30 | return -1; 31 | } 32 | 33 | LuaScriptManager manager(argv[1]); 34 | 35 | // Register name of above implemented callback in the Lua script 36 | char callbackName[] = "mysin"; 37 | manager.registerCallback(l_sin, callbackName); 38 | 39 | while(1) 40 | { 41 | double x = M_PI/3.0; 42 | double z; 43 | 44 | manager.callFunction("aLuaFunctionWithCallback", "d>d", x, &z); 45 | cout << "Called with parameters x=" << x << endl; 46 | cout << " returned value z=" << z << endl; 47 | 48 | // slow down loop 49 | usleep(500000); 50 | 51 | // reloading allows to modify Lua function during runtime 52 | manager.reload(); 53 | } 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /obcore/base/types.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * 3 | * Copyright (C) 2014 by TH-Nürnberg 4 | * Written by Christian Merkl 5 | * All Rights Reserved. 6 | * 7 | * This program is free software; you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation; either version 2 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License along 18 | * with this program; if not, write to the Free Software Foundation, Inc., 19 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 20 | * 21 | *********************************************************************************/ 22 | #ifndef ___TYPES_H___ 23 | #define ___TYPES_H___ 24 | 25 | namespace obvious { 26 | 27 | /* set here the used data type for floating point operation. */ 28 | #define _OBVIOUS_DOUBLE_PRECISION_ 1 29 | 30 | #if _OBVIOUS_DOUBLE_PRECISION_ 31 | typedef double obfloat; 32 | #else 33 | typedef float obfloat; 34 | #endif 35 | 36 | } // end namespace obvious 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /obcore/filter/FilterDistance.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FilterDistance.h 3 | * @autor christian 4 | * @date 10.12.2012 5 | * 6 | * 7 | */ 8 | 9 | #ifndef FILTERDISTANCE_H_ 10 | #define FILTERDISTANCE_H_ 11 | 12 | #include "obcore/filter/Filter.h" 13 | 14 | /** 15 | * @namespace obvious 16 | */ 17 | namespace obvious 18 | { 19 | /** 20 | * @class FilterDistance 21 | * @brief Abstract class for distance filters as @see EuclideanFilterD.h and 22 | * @see CartesianFilter.h 23 | */ 24 | class FilterDistance : public Filter 25 | { 26 | public: 27 | /** 28 | * Standard constructor with initialization 29 | */ 30 | FilterDistance() 31 | : _centroid(Point3D(0.0, 0.0, 0.0)) { } 32 | /** 33 | * virtual destructor 34 | */ 35 | virtual ~FilterDistance() { } 36 | /** 37 | * Function to set ne centroid for filtering 38 | * @param center center of filter 39 | */ 40 | virtual void setCentroid(const Point3D& center) = 0; 41 | /** 42 | * Function to get set centroid 43 | * @return _centroid 44 | */ 45 | const Point3D& getCentroid(void) { return _centroid; } 46 | protected: 47 | /** 48 | * Default function for derived classes to call 49 | * @param center center of filter 50 | */ 51 | virtual void setCentroidDefault(const Point3D& center) {_centroid = center; } 52 | Point3D _centroid; ///< centroid to filter 53 | }; 54 | 55 | }; // namespace 56 | 57 | #endif /* FILTERDISTANCE_H_ */ 58 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/RobotFootprintFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RobotFootprintFilter.cpp 3 | * 4 | * Created on: 20.09.2013 5 | * Author: chris 6 | */ 7 | 8 | 9 | #include "RobotFootprintFilter.h" 10 | 11 | namespace obvious 12 | { 13 | 14 | RobotFootprintFilter::RobotFootprintFilter(unsigned int dim, double maxRadius) 15 | { 16 | _dim = dim; 17 | _maxRadius = maxRadius; 18 | /*_offset = NULL; 19 | _x_lim = NULL; 20 | _y_lim = NULL; 21 | _z_lim = NULL;*/ 22 | }; 23 | 24 | /*RobotFootprintFilter::RobotFootprintFilter(const double* x_lim, const double* y_lim, const double* z_lim) 25 | { 26 | //_minRadius = 0.0; 27 | //_maxRadius = 0.0; 28 | //_offset = NULL; 29 | //_x_lim = NULL; 30 | //_y_lim = NULL; 31 | //_z_lim = NULL; 32 | }*/ 33 | 34 | RobotFootprintFilter::~RobotFootprintFilter() 35 | { 36 | 37 | } 38 | 39 | void RobotFootprintFilter::filter(double** scene, unsigned int size, bool* mask) 40 | { 41 | if(_dim==2) 42 | { 43 | double thresh = _maxRadius * _maxRadius; 44 | for (unsigned int i=0 ; i(p) < _maxRadius) 56 | mask[i] = false; 57 | } 58 | } 59 | } 60 | 61 | } 62 | -------------------------------------------------------------------------------- /applications/ndt_matching2D.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Sample application showing the usage of the OBVIOUS NDT implementation. 3 | * @author Stefan May 4 | * @date 23.08.2014 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "obcore/math/mathbase.h" 11 | #include "obcore/math/linalg/linalg.h" 12 | #include "obcore/base/Timer.h" 13 | #include "obvision/registration/ndt/Ndt.h" 14 | 15 | using namespace std; 16 | using namespace obvious; 17 | 18 | int main(int argc, char** argv) 19 | { 20 | // Model coordinates 21 | obvious::Matrix* M = new obvious::Matrix(100, 2); 22 | 23 | for(int i=0; i<100; i++) 24 | { 25 | double di = (double) i; 26 | (*M)(i,0) = sin(di/25.0); 27 | (*M)(i,1) = sin(di/10.0); 28 | } 29 | 30 | obvious::Matrix T = MatrixFactory::TransformationMatrix33(deg2rad(9.0), 0.4, 0.35); 31 | obvious::Matrix S = M->createTransform(T); 32 | 33 | Ndt* ndt = new Ndt(-1, 1, -1, 1); 34 | ndt->setModel(M); 35 | ndt->setScene(&S); 36 | 37 | double rms; 38 | unsigned int it; 39 | ndt->iterate(&rms, &it); 40 | obvious::Matrix F = ndt->getFinalTransformation(); 41 | F.invert(); 42 | cout << endl << "Error: " << rms << endl; 43 | cout << "Iterations: " << it << endl; 44 | 45 | cout << "Applied transformation:" << endl; 46 | T.print(); 47 | cout << endl << "Estimated transformation:" << endl; 48 | F.print(); 49 | 50 | delete ndt; 51 | 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /applications/lua_statemachine.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "obcore/statemachine/AgentModel.h" 5 | #include "obcore/statemachine/states/StateLua.h" 6 | 7 | using namespace std; 8 | using namespace obvious; 9 | 10 | /** 11 | * Example showing usage of LuaScriptManager: State machine 12 | * @author Stefan May 13 | * @date 17.6.2015 14 | */ 15 | int main(int argc, char* argv[]) 16 | { 17 | if(argc<3) 18 | { 19 | cout << "usage: " << argv[0] << " .../lua/state1.lua .../lua/state2.lua ..." << endl; 20 | return -1; 21 | } 22 | 23 | // The state machine might use a user-defined model. For that purpose use your own model class instances and pass them to your states. 24 | AgentModel model; 25 | 26 | // Instantiate generic agent. It processes activated states. You can use Lua states as well as pure C++ states (see statemachine_test). 27 | Agent agent; 28 | 29 | // Instantiate Lua states from passed file path parameters 30 | for(int i=1; i 5 | 6 | #include 7 | #include 8 | 9 | namespace obvious { 10 | 11 | class OpenNiDevice 12 | { 13 | public: 14 | 15 | enum Flag { 16 | Any = 0xff, 17 | Depth = 1, 18 | Color = 2, 19 | Ir = 4, 20 | All = Depth | Color | Ir 21 | }; 22 | 23 | OpenNiDevice(const Flag flags = Any, const std::string& deviceURI = std::string()); 24 | ~OpenNiDevice(void); 25 | 26 | Flag flags(void) const { return _flags; } 27 | bool init(void); 28 | bool grab(void); 29 | int width(void) const { return _width; } 30 | int height(void) const { return _height; } 31 | const std::vector& z(void) const { return _z; } 32 | const std::vector& coords(void) const { return _coords; } 33 | unsigned char* rgb(void) const { return _imgRgb; } 34 | 35 | private: 36 | openni::Status _status; 37 | openni::Device _device; 38 | openni::VideoStream _depth; 39 | openni::VideoStream _color; 40 | openni::VideoStream _ir; 41 | openni::VideoFrameRef _frameDepth; 42 | openni::VideoFrameRef _frameColor; 43 | openni::VideoFrameRef _frameIr; 44 | 45 | Flag _flags; 46 | int _width; 47 | int _height; 48 | std::vector _z; 49 | std::vector _coords; 50 | unsigned char* _imgRgb; 51 | unsigned char* _imgIr; 52 | }; 53 | 54 | } // end namespace obvious 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StateLua.h: -------------------------------------------------------------------------------- 1 | #ifndef STATELUA_H_ 2 | #define STATELUA_H_ 3 | 4 | #include 5 | #include "obcore/scripting/LuaScriptManager.h" 6 | 7 | namespace obvious 8 | { 9 | 10 | /** 11 | * @class StateLua 12 | * @brief Generic state for interfacing Lua scripts 13 | * @author Stefan May 14 | */ 15 | class StateLua: public StateBaseModel 16 | { 17 | public: 18 | 19 | /** 20 | * Constructor 21 | */ 22 | StateLua(AgentModel* model, const char* filepath); 23 | 24 | /** 25 | * Destructor 26 | */ 27 | virtual ~StateLua(); 28 | 29 | /** 30 | * Set automatic reload flag. If true, the lua script is reloaded every cycle. 31 | * @param autoReload reload flag 32 | */ 33 | void setAutoReload(bool autoReload); 34 | 35 | /** 36 | * Get automatic reload flag. If true, the lua script is reloaded every cycle. 37 | * @param return reload flag 38 | */ 39 | bool getAutoReload(); 40 | 41 | /** 42 | * Called once when registered 43 | */ 44 | virtual void onSetup(); 45 | 46 | /** 47 | * Called once when activated 48 | */ 49 | virtual void onEntry(); 50 | 51 | /** 52 | * Process method (step-wise, never block this method) 53 | */ 54 | virtual void onActive(); 55 | 56 | /** 57 | * Called once when left 58 | */ 59 | virtual void onExit(); 60 | 61 | private: 62 | 63 | void reload(); 64 | 65 | LuaScriptManager* _manager; 66 | 67 | bool _autoReload; 68 | 69 | bool _loadInThisCycle; 70 | 71 | }; 72 | 73 | } /* namespace obvious */ 74 | 75 | #endif /* STATELUA_H_ */ 76 | -------------------------------------------------------------------------------- /applications/kinect_perspective.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Sample application showing the reprojection of Kinect data to depth image plane. 3 | * @author Stefan May 4 | * @date 22.04.2011 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "obcore/base/CartesianCloudFactory.h" 11 | #include "obcore/base/tools.h" 12 | #include "obcore/math/geometry.h" 13 | #include "obgraphic/VtkCloud.h" 14 | 15 | using namespace obvious; 16 | 17 | 18 | int main(int argc, char** argv) 19 | { 20 | 21 | if(argc != 4) 22 | { 23 | printf("usage: %s \n", argv[0]); 24 | exit(1); 25 | } 26 | 27 | //CartesianCloud3D* cloud = CartesianCloudFactory::load(argv[1], eFormatAscii); 28 | 29 | VtkCloud* vcloud = VtkCloud::load(argv[1], VTKCloud_AUTO); 30 | vcloud->removeInvalidPoints(); 31 | CartesianCloud3D* cloud = new CartesianCloud3D(vcloud->getSize(), true); 32 | vcloud->copyData(cloud->getCoords(), cloud->getNormals(), cloud->getColors()); 33 | 34 | double perspective[12] = {585.05108211, 0.00000000, 315.83800193, 0., 0.00000000, 585.05108211, 242.94140713, 0., 0.00000000, 0.00000000, 1.00000000, 0.}; 35 | 36 | Matrix P(3,4); 37 | P.setData(perspective); 38 | 39 | unsigned char *buf = (unsigned char *) malloc(3 * 640 * 480 * sizeof(unsigned char)); 40 | unsigned char *msk = (unsigned char *) malloc(640 * 480 * sizeof(unsigned char)); 41 | cloud->createProjection(buf, msk, &P, 640, 480); 42 | serializePPM(argv[2], buf, 640, 480); 43 | serializePBM(argv[3], msk, 640, 480); 44 | free(buf); 45 | free(msk); 46 | } 47 | -------------------------------------------------------------------------------- /obcore/grid/GradientGrid.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GradientGrid.h 3 | * @author Christian Pfitzner 4 | * @date 06.01.2013 5 | * 6 | * 7 | */ 8 | 9 | #ifndef GRADIENTGRID_H_ 10 | #define GRADIENTGRID_H_ 11 | 12 | #include "obcore/grid/Grid2D.h" 13 | 14 | /** 15 | * @namespace obvious 16 | */ 17 | namespace obvious { 18 | /** 19 | * @class GradientGrid 20 | * @brief Grid to estimate gradiend in x and y direction of a given height map 21 | */ 22 | class GradientGrid : public Grid2D 23 | { 24 | public: 25 | /** 26 | * Standard constructor 27 | * @param[in] resolution length of one grid cell 28 | * @param[in] length length of grid 29 | * @param[in] width width of grid 30 | */ 31 | GradientGrid(const double& resolution, const double& length, const double& width) 32 | : Grid2D(resolution, length, width, 2) { } 33 | /** 34 | * Default destructor 35 | */ 36 | virtual ~GradientGrid(void) { } 37 | /** 38 | * Function to get height map 39 | * @param[in] heightGrid height grid of a cloud 40 | * @return TRUE if no error occurs @see SUCCESFUL 41 | */ 42 | SUCCESFUL gradient2Grid(const MatD& heigthMat); 43 | /** 44 | * Function to return unsigned char image for visualization 45 | * @param img allocated unsigned char array 46 | */ 47 | unsigned char* getImageOfGrid(void); 48 | private: 49 | /** 50 | * Function to return map as image with gradients 51 | * @param img 52 | */ 53 | unsigned char* getGradientMap(void); 54 | enum EnumChannel{GRADIENT_X, GRADIENT_Y}; 55 | 56 | }; 57 | 58 | } // namespace 59 | 60 | #endif /* GRADIENTGRID_H_ */ 61 | -------------------------------------------------------------------------------- /obvision/reconstruct/space/SensorPolar3D.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSOR_POLAR_3D_H 2 | #define SENSOR_POLAR_3D_H 3 | 4 | #include "obvision/reconstruct/Sensor.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class SensorPolar3D 11 | * @brief Generic class for 3D measurement units using polar sampling 12 | * @author Stefan May 13 | */ 14 | class SensorPolar3D : public Sensor 15 | { 16 | public: 17 | 18 | /** 19 | * Standard constructor 20 | * @param[in] beams number of beams in scanning plane 21 | * @param[in] thetaRes angular resolution, i.e. angle between beams in rad 22 | * @param[in] thetaMin minimum angle from which beams are counted positive counter-clockwisely (rad) 23 | * @param[in] planes scanning planes per half revolution 24 | */ 25 | SensorPolar3D(unsigned int beams, double thetaRes, double thetaMin, unsigned int planes=360, double maxRange=INFINITY, double minRange=0.0, double lowReflectivityRange=INFINITY); 26 | 27 | /** 28 | * Destructor 29 | */ 30 | ~SensorPolar3D(); 31 | 32 | /** 33 | * Parallel version of back projection 34 | * @param[in] M matrix of homogeneous 3D coordinates 35 | * @param[out] indices vector of beam indices 36 | * @param[in] T temporary transformation matrix of coordinates 37 | */ 38 | void backProject(Matrix* M, int* indices, Matrix* T=NULL); 39 | 40 | void setDistanceMap(vector phi, vector dist); 41 | 42 | private: 43 | 44 | double _thetaRes; 45 | 46 | double _phiRes; 47 | 48 | double _thetaMin; 49 | 50 | double _thetaLowerBound; 51 | 52 | double _thetaUpperBound; 53 | 54 | double** _distanceMap; 55 | 56 | int** _indexMap; 57 | 58 | }; 59 | 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /obvision/planning/AStar.h: -------------------------------------------------------------------------------- 1 | /* 2 | * AStar.h 3 | * 4 | * Created on: 03.11.2014 5 | * Author: Jon Martin and Stefan May 6 | */ 7 | 8 | #ifndef ASTAR_H_ 9 | #define ASTAR_H_ 10 | 11 | #include 12 | #include 13 | 14 | #include "obvision/planning/Obstacle.h" 15 | #include "obvision/planning/AStarMap.h" 16 | 17 | namespace obvious 18 | { 19 | 20 | class AStar 21 | { 22 | 23 | public: 24 | 25 | /** 26 | * Plan path giving start and target indices 27 | * @param map map 28 | * @param start pixel coordinates of starting point 29 | * @param target pixel coordinates of target 30 | * @param penalty use directional change penalty 31 | * @return path 32 | */ 33 | static std::vector pathFind(AStarMap* map, const Pixel start, const Pixel target, const bool penalty); 34 | 35 | /** 36 | * Plan path giving start and target coordinates 37 | * @param map map 38 | * @param coordStart coordinates of starting point 39 | * @param coordTarget coordinates of target 40 | * @param penalty use directional change penalty 41 | * @return path 42 | */ 43 | static std::vector pathFind(AStarMap* map, const Point2D coordStart, const Point2D coordTarget, const bool penalty, const Point2D* const offset = NULL); 44 | 45 | /** 46 | * Convert row/col indices to cell index 47 | * @param x column 48 | * @param y row 49 | * @param width width of grid, i.e., number of columns 50 | */ 51 | static inline unsigned int toId(unsigned int x, unsigned int y, unsigned int width) 52 | { 53 | return y*width+x; 54 | } 55 | 56 | private: 57 | 58 | }; 59 | 60 | } /* namespace obvious */ 61 | #endif /* ASTAR_H_ */ 62 | -------------------------------------------------------------------------------- /obcore/grid/HeightGrid.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file HeightGrid.h 3 | * @author Christian Pfitzner 4 | * @date 06.01.2013 5 | * 6 | * 7 | */ 8 | 9 | #ifndef HEIGHTGRID_H_ 10 | #define HEIGHTGRID_H_ 11 | 12 | #include "obcore/grid/Grid2D.h" 13 | /** 14 | * @namespace obvious 15 | */ 16 | namespace obvious { 17 | /** 18 | * @class HeightGrid 19 | * @brief Class for height maps 20 | * 21 | * This class builds height maps out of a given point cloud and is derived 22 | * from Grid2D 23 | * @see Grid2D 24 | */ 25 | class HeightGrid : public Grid2D 26 | { 27 | public: 28 | /** 29 | * Standard constructor 30 | * @param[in] resolution length of one grid cell 31 | * @param[in] length length of grid 32 | * @param[in] width width of grid 33 | */ 34 | HeightGrid(const double& resolution, const double& length, const double& width) 35 | : Grid2D(resolution, length, width, 1) { } 36 | /** 37 | * Default destructor 38 | */ 39 | virtual ~HeightGrid(void) { } 40 | /** 41 | * Function to get height map 42 | * @param[in] cloud cloud with points in double array 43 | * @param[in] size number of points in cloud 44 | * @return TRUE if no error occurs @see SUCCESFUL 45 | */ 46 | SUCCESFUL height2Grid(double* cloud, unsigned int size); 47 | /** 48 | * Function to return unsigned char image for visualization 49 | * @param[in] img allocated unsigned char array 50 | */ 51 | void getImageOfGrid(unsigned char* img); 52 | private: 53 | /** 54 | * Function to return map as image with heights 55 | * @param img 56 | */ 57 | void getHeightMap(unsigned char* img); 58 | 59 | }; 60 | 61 | } 62 | 63 | 64 | 65 | #endif /* HEIGHTGRID_H_ */ 66 | -------------------------------------------------------------------------------- /obcore/math/linalg/eigen/Vector.h: -------------------------------------------------------------------------------- 1 | #ifndef VECTOR_H__ 2 | #define VECTOR_H__ 3 | 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | /** 10 | * @namespace obvious 11 | */ 12 | namespace obvious 13 | { 14 | 15 | /** 16 | * @class Vector 17 | * @brief Vector abstraction layer of GSL 18 | * @author Stefan May 19 | */ 20 | class Vector 21 | { 22 | friend class Matrix; 23 | friend class VectorView; 24 | 25 | public: 26 | /** 27 | * Constructor 28 | * @param size number of vector elements 29 | * @param data data buffer to be copied 30 | */ 31 | Vector(unsigned int size, double* data = NULL); 32 | 33 | /** 34 | * Copy constructor 35 | * @param V vector to be copied 36 | */ 37 | Vector(const Vector &V); 38 | 39 | /** 40 | * Destructor 41 | */ 42 | ~Vector(); 43 | 44 | /** 45 | * Assignment operator 46 | * @param V vector assigned to this one 47 | * @return this vector instance 48 | */ 49 | Vector &operator = (const Vector &V); 50 | 51 | /** 52 | * Element accessor 53 | * @param i element index 54 | * @return element 55 | */ 56 | double& operator () (unsigned int i); 57 | 58 | /** 59 | * Property accessor 60 | * @return number of vector elements 61 | */ 62 | unsigned int getSize(); 63 | 64 | /** 65 | * Set data from array 66 | * @param array source array 67 | */ 68 | void setData(const double* array); 69 | 70 | /** 71 | * Set all vector elements to zero 72 | */ 73 | void setZero(); 74 | 75 | /** 76 | * Print matrix to output stream 77 | */ 78 | void print(); 79 | 80 | private: 81 | 82 | /** 83 | * Internal GSL representation 84 | */ 85 | Eigen::VectorXd _V; 86 | 87 | }; 88 | 89 | } 90 | 91 | #endif //VECTOR_H 92 | -------------------------------------------------------------------------------- /obcore/math/linalg/gsl/Vector.h: -------------------------------------------------------------------------------- 1 | #ifndef VECTOR_H__ 2 | #define VECTOR_H__ 3 | 4 | #include 5 | 6 | #include 7 | 8 | using namespace std; 9 | 10 | /** 11 | * @namespace obvious 12 | */ 13 | namespace obvious 14 | { 15 | 16 | /** 17 | * @class Vector 18 | * @brief Vector abstraction layer of GSL 19 | * @author Stefan May 20 | */ 21 | class Vector 22 | { 23 | friend class Matrix; 24 | friend class VectorView; 25 | 26 | public: 27 | /** 28 | * Constructor 29 | * @param size number of vector elements 30 | * @param data data buffer to be copied 31 | */ 32 | Vector(unsigned int size, double* data = NULL); 33 | 34 | /** 35 | * Copy constructor 36 | * @param V vector to be copied 37 | */ 38 | Vector(const Vector &V); 39 | 40 | /** 41 | * Destructor 42 | */ 43 | ~Vector(); 44 | 45 | /** 46 | * Assignment operator 47 | * @param V vector assigned to this one 48 | * @return this vector instance 49 | */ 50 | Vector &operator = (const Vector &V); 51 | 52 | /** 53 | * Element accessor 54 | * @param i element index 55 | * @return element 56 | */ 57 | double& operator () (unsigned int i); 58 | 59 | /** 60 | * Property accessor 61 | * @return number of vector elements 62 | */ 63 | unsigned int getSize(); 64 | 65 | /** 66 | * Set data from array 67 | * @param array source array 68 | */ 69 | void setData(const double* array); 70 | 71 | /** 72 | * Set all vector elements to zero 73 | */ 74 | void setZero(); 75 | 76 | /** 77 | * Print matrix to output stream 78 | */ 79 | void print(); 80 | 81 | private: 82 | 83 | /** 84 | * Internal GSL representation 85 | */ 86 | gsl_vector* _V; 87 | 88 | }; 89 | 90 | } 91 | 92 | #endif //VECTOR_H 93 | -------------------------------------------------------------------------------- /obcore/filter/EuclideanFilterVecD.h: -------------------------------------------------------------------------------- 1 | /* 2 | * EuclideanFilterVecD.h 3 | * 4 | * Created on: 30.11.2012 5 | * Author: phil 6 | */ 7 | 8 | #ifndef EUCLIDEANFILTERVECD_H_ 9 | #define EUCLIDEANFILTERVECD_H_ 10 | 11 | #include "obcore/filter/Filter.h" 12 | #include 13 | 14 | namespace obvious 15 | { 16 | 17 | using std::vector; 18 | 19 | /** 20 | * @class EuclideanFilterVecD 21 | * Filter to push values from the input into the output buffer. 22 | * Uses the vector class of the stl. 23 | * Values which amount is bigger than a threshold, won't be copied. 24 | */ 25 | class EuclideanFilterVecD : public Filter 26 | { 27 | public: 28 | EuclideanFilterVecD(void) 29 | : _threshold(0.0), 30 | _output(NULL), 31 | _input(NULL) { } 32 | virtual ~EuclideanFilterVecD(){ } 33 | 34 | 35 | /** 36 | * Function to start the Filter 37 | */ 38 | FILRETVAL applyFilter(void); 39 | /** 40 | * Function to set the value of the threshold parameter 41 | * @param[in] val new threshold value 42 | */ 43 | void setThreshold(const double val){_threshold=val;} 44 | 45 | /** 46 | * Function to set the pointer to the input vector 47 | * @param[in] vec Adress of the input vector 48 | */ 49 | void setInput(vector *vec){_input=vec;} 50 | 51 | /** 52 | * Function to set the pointer to the output vector 53 | * @param[in] vec Adress of the output vector 54 | */ 55 | void setOutput(vector *vec){_output=vec;} 56 | private: 57 | double _threshold; ///< Threshold parameter of the filter 58 | vector *_input; ///< Adress of the input vector 59 | vector *_output; ///< Adress of the output vector 60 | }; 61 | 62 | } 63 | 64 | #endif /* EUCLIDEANFILTERVECD_H_ */ 65 | -------------------------------------------------------------------------------- /applications/obvious3D_map.cpp: -------------------------------------------------------------------------------- 1 | #include "obcore/base/CartesianCloud.h" 2 | #include "obcore/base/CartesianCloudFactory.h" 3 | #include "obgraphic/Obvious3D.h" 4 | 5 | #include 6 | 7 | using namespace obvious; 8 | 9 | int main(int argc, char *argv[]) 10 | { 11 | if(argc<2) 12 | { 13 | cout << "usage: " << argv[0] << " [inc]" << endl; 14 | cout << " inc (optional): incremental poses (default: 0)" << endl; 15 | } 16 | 17 | ifstream file; 18 | file.open(argv[1], ios::in); 19 | if(!file) 20 | { 21 | cout << "File not found: " << argv[1] << endl; 22 | return -1; 23 | } 24 | 25 | Obvious3D* viewer = new Obvious3D(); 26 | 27 | bool inc = 0; 28 | if(argc==3) 29 | inc = atol(argv[2]); 30 | 31 | Matrix T(4, 4); 32 | T.setIdentity(); 33 | 34 | while (1) 35 | { 36 | char filename[1024]; 37 | file.getline(filename, 1024); 38 | if(!file.good()) break; 39 | 40 | cout << filename << endl; 41 | 42 | CartesianCloud3D* cloud = CartesianCloudFactory::load(filename, eFormatAscii); 43 | cloud->removeInvalidPoints(); 44 | 45 | double Tdata[16]; 46 | Matrix Tcur(4, 4); 47 | std::string line; 48 | std::getline(file, line); 49 | std::istringstream iss(line); 50 | for(int i=0; i<16; i++) 51 | iss >> Tdata[i]; 52 | Tcur.setData(Tdata); 53 | if(inc) 54 | T = T * Tcur; 55 | else 56 | T = Tcur; 57 | cloud->transform(&T); 58 | Matrix* coords = cloud->getCoords(); 59 | VtkCloud* vcloud = new VtkCloud(); 60 | vcloud->setCoords((*coords)[0], coords->getRows(), 3, NULL); 61 | vcloud->setColors(cloud->getColors(), coords->getRows(), 3); 62 | 63 | viewer->addCloud(vcloud); 64 | } 65 | 66 | viewer->startRendering(); 67 | } 68 | -------------------------------------------------------------------------------- /obcore/base/Point.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * 3 | * Copyright (C) 2014 by TH-Nürnberg 4 | * Written by Christian Merkl 5 | * All Rights Reserved. 6 | * 7 | * This program is free software; you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation; either version 2 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License along 18 | * with this program; if not, write to the Free Software Foundation, Inc., 19 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 20 | * 21 | *********************************************************************************/ 22 | #ifndef ___POINT_H___ 23 | #define ___POINT_H___ 24 | 25 | #include "obcore/base/types.h" 26 | 27 | #include 28 | 29 | namespace obvious { 30 | 31 | struct Pixel 32 | { 33 | unsigned int u; 34 | unsigned int v; 35 | }; 36 | 37 | struct Point 38 | { 39 | obfloat x; 40 | obfloat y; 41 | obfloat z; 42 | }; 43 | 44 | struct Point2D 45 | { 46 | obfloat x; 47 | obfloat y; 48 | }; 49 | 50 | struct PointRgb : public Point 51 | { 52 | unsigned char r; 53 | unsigned char g; 54 | unsigned char b; 55 | }; 56 | 57 | struct PointRgbT : public PointRgb 58 | { 59 | uint16_t t; 60 | }; 61 | 62 | } // end namespace obvious 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/DistanceFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "DistanceFilter.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace obvious 8 | { 9 | 10 | 11 | DistanceFilter::DistanceFilter(double maxdist, double mindist, unsigned int iterations) 12 | { 13 | _maxDistSqr = maxdist * maxdist; 14 | _minDistSqr = mindist * mindist; 15 | _distSqr = _maxDistSqr; 16 | double it = (double)(iterations - 1); 17 | if(iterations < 1) it = 1.0; 18 | // i'th root 19 | _multiplier = pow((mindist / maxdist), 1.0 / it); 20 | }; 21 | 22 | DistanceFilter::~DistanceFilter() 23 | { 24 | 25 | }; 26 | 27 | void DistanceFilter::reset() 28 | { 29 | _distSqr = _maxDistSqr; 30 | } 31 | 32 | void DistanceFilter::filter(double** model, 33 | double** scene, 34 | vector* pairs, 35 | vector* distancesSqr, 36 | vector* fpairs, 37 | vector* fdistancesSqr, 38 | vector* nonPairs) 39 | { 40 | if(!_active) 41 | { 42 | *fpairs = *pairs; 43 | *fdistancesSqr = *distancesSqr; 44 | return; 45 | } 46 | 47 | fpairs->clear(); 48 | fdistancesSqr->clear(); 49 | 50 | for(unsigned int p=0; psize(); p++) 51 | { 52 | if((*distancesSqr)[p] <= _distSqr) 53 | { 54 | fpairs->push_back((*pairs)[p]); 55 | fdistancesSqr->push_back((*distancesSqr)[p]); 56 | } 57 | else 58 | { 59 | nonPairs->push_back((*pairs)[p].indexSecond); 60 | } 61 | } 62 | _distSqr *= _multiplier; 63 | if(_distSqr < _minDistSqr) _distSqr = _minDistSqr; 64 | } 65 | 66 | } 67 | -------------------------------------------------------------------------------- /obcore/grid/HeightGrid.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file HeightGrid.cpp 3 | * @autor christian 4 | * @date 06.01.2013 5 | * 6 | * 7 | */ 8 | 9 | #include "obcore/grid/HeightGrid.h" 10 | #include "obcore/Point3D.h" 11 | 12 | using namespace obvious; 13 | 14 | SUCCESFUL HeightGrid::height2Grid(double* cloud, unsigned int size) 15 | { 16 | initGrid(); 17 | for(unsigned int i=0; iat(x,y,0) < cloud[i+Y]) 27 | _grid->at(x,y,0) = cloud[i+Y]; 28 | } 29 | } 30 | _pointsEstimated = false; 31 | return(ALRIGHT); 32 | } 33 | 34 | void HeightGrid::getImageOfGrid(unsigned char *img) 35 | { 36 | getHeightMap(img); 37 | } 38 | 39 | //~~~~~~~~~~~~ Private ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 40 | void HeightGrid::getHeightMap(unsigned char *img) 41 | { 42 | unsigned char minValue = 255; 43 | unsigned char maxValue = 0; 44 | 45 | // estimate min max value für maximum color range 46 | for(unsigned int x=0 ; x<_cols ; x++) { 47 | for(unsigned int y=0 ; y<_rows ; y++) 48 | { 49 | if(_grid->at(x,y) > maxValue) 50 | maxValue = _grid->at(x,y); 51 | if(_grid->at(x,y) < minValue) 52 | minValue = _grid->at(x,y); 53 | } 54 | } 55 | 56 | double range = maxValue - minValue; 57 | 58 | // checkout data from grid to image 59 | for(unsigned int x=0 ; x<_cols ; x++) 60 | for(unsigned int y=0 ; y<_rows ; y++) 61 | _img[x*_rows + y] = (unsigned char)(_grid->at(x,y) - minValue)/range*255; 62 | } 63 | -------------------------------------------------------------------------------- /obcore/math/PID_Controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PID_Controller.cpp 3 | * @author Christian Pfitzner 4 | * @date 09.11.2012 5 | * 6 | * 7 | */ 8 | 9 | #include "PID_Controller.h" 10 | using namespace obvious; 11 | 12 | PID_Controller::PID_Controller(void) 13 | { 14 | _p = 0; 15 | _i = 0; 16 | _d = 0; 17 | _awu = 0; 18 | _maxOutput = -1e6; 19 | _minOutput = 1e6; 20 | _setValue = 0; 21 | _isValue = 0; 22 | _debug = false; 23 | 24 | _timer.start(); 25 | } 26 | 27 | double PID_Controller::control(const double& isValue) 28 | { 29 | static double oldError = 0.0; 30 | 31 | // determine deltaT in seconds 32 | const double deltaT = _timer.reset(); 33 | 34 | _isValue = isValue; 35 | 36 | double error = _setValue - _isValue; 37 | 38 | double p_ctrl_output = error * _p; 39 | double i_ctrl_output = _i * _integrator.integrate(error, deltaT); 40 | 41 | // ensure numerical stability 42 | if(deltaT<1e-6) 43 | { 44 | return (p_ctrl_output + i_ctrl_output); 45 | } 46 | 47 | double d_ctrl_output = (error - oldError) / deltaT * _d; 48 | 49 | double ctrl_output = p_ctrl_output + i_ctrl_output + d_ctrl_output; 50 | 51 | // check limits 52 | if (ctrl_output >= _maxOutput) 53 | ctrl_output = _maxOutput; 54 | if (ctrl_output <= _minOutput) 55 | ctrl_output = _minOutput; 56 | 57 | if (_debug) 58 | { 59 | std::cout << "Error : " << error << std::endl; 60 | std::cout << "p value: " << p_ctrl_output << std::endl; 61 | std::cout << "i value: " << i_ctrl_output << std::endl; 62 | std::cout << "d value: " << d_ctrl_output << std::endl; 63 | std::cout << "Output: " << ctrl_output << std::endl; 64 | } 65 | return (ctrl_output); 66 | } 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /obcore/base/System.inl: -------------------------------------------------------------------------------- 1 | template 2 | void System::allocate (unsigned int rows, unsigned int cols, T** &array2D) 3 | { 4 | array2D = new T*[rows]; 5 | array2D[0] = new T[rows*cols]; 6 | for (unsigned int row = 1; row < rows; row++) 7 | { 8 | array2D[row] = &array2D[0][cols*row]; 9 | } 10 | } 11 | 12 | template 13 | void System::deallocate (T**& array2D) 14 | { 15 | delete[] array2D[0]; 16 | delete[] array2D; 17 | array2D = 0; 18 | } 19 | 20 | template 21 | void System::copy (unsigned int rows, unsigned int cols, T** &src, T** &dst) 22 | { 23 | memcpy(dst[0], src[0], rows*cols*sizeof(T)); 24 | } 25 | 26 | template 27 | void System::initZero(unsigned int rows, unsigned int cols, T** buf) 28 | { 29 | memset(buf[0], 0, rows*cols*sizeof(T)); 30 | } 31 | 32 | template 33 | void System::allocate (unsigned int rows, unsigned int cols, unsigned int slices, T*** &array3D) 34 | { 35 | array3D = new T**[rows]; 36 | for (unsigned int row = 0; row < rows; row++) 37 | System::allocate(cols, slices, array3D[row]); 38 | } 39 | 40 | template 41 | void System::deallocate (T***& array3D) 42 | { 43 | delete[] array3D[0][0]; 44 | delete[] array3D[0]; 45 | delete[] array3D; 46 | array3D = 0; 47 | } 48 | 49 | template 50 | void System::copy (unsigned int rows, unsigned int cols, unsigned int slices, T*** &src, T*** &dst) 51 | { 52 | for (unsigned int row = 0; row < rows; row++) 53 | System::memcpy(cols, slices, src[row], dst[row]); 54 | } 55 | 56 | template 57 | void System::initZero(unsigned int rows, unsigned int cols, unsigned int slices, T*** buf) 58 | { 59 | for(unsigned int i=0; i _threshold) { 35 | dPtr += Point3D::sizeP; 36 | } 37 | else { 38 | for(unsigned int j=0 ; j 2 | #include "obgraphic/Obvious3D.h" 3 | #include "obdevice/KinectPlayback.h" 4 | 5 | using namespace std; 6 | using namespace obvious; 7 | 8 | Obvious3D* _viewer; 9 | VtkCloud* _cloud; 10 | KinectPlayback* _kinect; 11 | 12 | class vtkTimerCallback : public vtkCommand 13 | { 14 | public: 15 | static vtkTimerCallback *New() 16 | { 17 | vtkTimerCallback *cb = new vtkTimerCallback; 18 | return cb; 19 | } 20 | 21 | virtual void Execute(vtkObject *vtkNotUsed(caller), unsigned long eventId, void *vtkNotUsed(callData)) 22 | { 23 | if(!_kinect->eof()) 24 | { 25 | if(_kinect->grab()) 26 | { 27 | int size = _kinect->getRows() * _kinect->getCols(); 28 | _cloud->setCoords(_kinect->getCoords(), size, 3); 29 | _cloud->setColors(_kinect->getRGB(), size, 3); 30 | _viewer->update(); 31 | } 32 | } 33 | } 34 | 35 | private: 36 | 37 | }; 38 | 39 | void resetFilestream() 40 | { 41 | _kinect->reset(); 42 | } 43 | 44 | int main(int argc, char* argv[]) 45 | { 46 | if(argc!=2) 47 | { 48 | cout << "usage: " << argv[0] << " " << endl; 49 | return -1; 50 | } 51 | 52 | _kinect = new KinectPlayback(argv[1]); 53 | _cloud = new VtkCloud(); 54 | _viewer = new Obvious3D(); 55 | _viewer->registerKeyboardCallback("space", resetFilestream); 56 | _viewer->addCloud(_cloud); 57 | 58 | vtkSmartPointer cb = vtkSmartPointer::New(); 59 | vtkSmartPointer interactor = _viewer->getWindowInteractor(); 60 | interactor->AddObserver(vtkCommand::TimerEvent, cb); 61 | 62 | interactor->CreateRepeatingTimer(30); 63 | _viewer->startRendering(); 64 | 65 | delete _cloud; 66 | delete _kinect; 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /applications/lua_read_config.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "obcore/scripting/LuaScriptManager.h" 4 | 5 | using namespace std; 6 | using namespace obvious; 7 | 8 | /** 9 | * Example showing usage of LuaScriptManager: Reading Lua tables 10 | * @author Stefan May 11 | * @date 16.6.2015 12 | */ 13 | int main(int argc, char* argv[]) 14 | { 15 | if(argc!=2) 16 | { 17 | cout << "usage: " << argv[0] << " .../lua/config.lua" << endl; 18 | return -1; 19 | } 20 | 21 | LuaScriptManager manager(argv[1]); 22 | 23 | double value1, value2; 24 | int value3; 25 | bool value4; 26 | const char * value5; 27 | 28 | vector keys; 29 | while(1) 30 | { 31 | keys.clear(); 32 | keys.push_back("value1"); 33 | keys.push_back("value2"); 34 | if(manager.readTable("group1", keys, "dd", &value1, &value2)) 35 | { 36 | cout << "--- Table 1 ---" << endl; 37 | cout << "Value read: " << value1 << endl; 38 | cout << "Value read: " << value2 << endl; 39 | } 40 | else 41 | { 42 | cout << "Reading Table 1 failed" << endl; 43 | } 44 | 45 | keys.clear(); 46 | keys.push_back("value3"); 47 | keys.push_back("value4"); 48 | keys.push_back("value5"); 49 | if(manager.readTable("group2", keys, "ibs", &value3, &value4, &value5)) 50 | { 51 | cout << "--- Table 2 ---" << endl; 52 | cout << "Value read: " << value3 << endl; 53 | cout << "Value read: " << value4 << endl; 54 | cout << "Value read: " << value5 << endl; 55 | } 56 | else 57 | { 58 | cout << "Reading Table 2 failed" << endl; 59 | } 60 | 61 | // slow down loop 62 | usleep(500000); 63 | 64 | // reloading allows to modify table during runtime 65 | manager.reload(); 66 | } 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/assignbase.h: -------------------------------------------------------------------------------- 1 | #ifndef ASSIGNBASE_H 2 | #define ASSIGNBASE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | /** 9 | * @namespace obvious 10 | */ 11 | namespace obvious 12 | { 13 | 14 | /** 15 | * @struct TdCartesianPoint 16 | * @brief Represents a single point in Cartesian space 17 | * @author Stefan May and Dirk Holz 18 | **/ 19 | typedef double* TdCartesianPoint; 20 | 21 | /** 22 | * @struct StrCartesianPair 23 | * @brief Represents a point pair in Cartesian space 24 | * @author Stefan May and Dirk Holz 25 | **/ 26 | struct StrCartesianPair 27 | { 28 | /** first point's coordinates */ 29 | TdCartesianPoint first; 30 | /** second point's coordinates */ 31 | TdCartesianPoint second; 32 | }; 33 | 34 | /** 35 | * @struct StrCartesianIndexPair 36 | * @brief Representation of one pair of point indices 37 | * @author Stefan May 38 | */ 39 | struct StrCartesianIndexPair 40 | { 41 | /** index of first point */ 42 | unsigned int indexFirst; 43 | /** index of second point */ 44 | unsigned int indexSecond; 45 | }; 46 | 47 | /** 48 | * @struct StrCartesianIndexDistancePair 49 | * @brief Represents a point pair and the euclidian distance in between 50 | * @author Markus Kühn 51 | */ 52 | struct StrCartestianIndexDistancePair 53 | { 54 | /** index of first point*/ 55 | unsigned int indexFirst; 56 | /** index of second point*/ 57 | unsigned int indexSecond; 58 | /** absolute distance */ 59 | double dist; 60 | 61 | /** Compare function according to distance, First is smaller second*/ 62 | static bool pairCompareDist(const StrCartestianIndexDistancePair& firstElem, const StrCartestianIndexDistancePair& secondElem) { 63 | return firstElem.dist < secondElem.dist; 64 | 65 | } 66 | }; 67 | 68 | } 69 | 70 | #endif /* ASSIGNBASE_H */ 71 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/AnnPairAssignment.cpp: -------------------------------------------------------------------------------- 1 | #include "AnnPairAssignment.h" 2 | #include "obcore/base/System.h" 3 | #include "obcore/base/tools.h" 4 | 5 | namespace obvious 6 | { 7 | 8 | void AnnPairAssignment::init() 9 | { 10 | _tree = NULL; 11 | } 12 | 13 | AnnPairAssignment::~AnnPairAssignment() 14 | { 15 | if(_tree) 16 | { 17 | delete _tree; 18 | _tree = NULL; 19 | annClose(); 20 | } 21 | } 22 | 23 | void AnnPairAssignment::setMaxVisitPoints(unsigned int visitPoints) 24 | { 25 | annMaxPtsVisit(visitPoints); 26 | } 27 | 28 | void AnnPairAssignment::setModel(double** model, int size) 29 | { 30 | 31 | if(_tree) 32 | { 33 | delete _tree; 34 | _tree = NULL; 35 | } 36 | _model = model; 37 | _tree = new ANNkd_tree( // build search structure 38 | model, // the data points 39 | size, // number of points 40 | _dimension); // dimension of space 41 | } 42 | 43 | void AnnPairAssignment::determinePairs(double** scene, bool* mask, int size) 44 | { 45 | ANNidxArray anIdx; // near neighbor indices 46 | ANNdistArray adDists; // near neighbor distances 47 | anIdx = new ANNidx[1]; 48 | adDists = new ANNdist[1]; 49 | 50 | double dErr = 0.0; 51 | 52 | for(int i = 0; i < size; i++) 53 | { 54 | if(mask[i]==1) 55 | { 56 | 57 | _tree->annkSearch( 58 | scene[i], // query point 59 | 1, // number of near neighbors to find 60 | anIdx, // nearest neighbor array (modified) 61 | adDists, // dist to near neighbors (modified) 62 | dErr); // error bound 63 | addPair(anIdx[0], i, adDists[0]); 64 | } 65 | else 66 | { 67 | addNonPair(i); 68 | } 69 | } 70 | delete[] anIdx; 71 | delete[] adDists; 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/NaboPairAssignment.cpp: -------------------------------------------------------------------------------- 1 | #include "NaboPairAssignment.h" 2 | #include "obcore/base/System.h" 3 | #include "obcore/base/tools.h" 4 | 5 | namespace obvious 6 | { 7 | 8 | void NaboPairAssignment::init() 9 | { 10 | _nns = NULL; 11 | } 12 | 13 | NaboPairAssignment::~NaboPairAssignment() 14 | { 15 | if(_nns) 16 | { 17 | delete _nns; 18 | _nns = NULL; 19 | } 20 | } 21 | 22 | void NaboPairAssignment::setModel(double** model, int size) 23 | { 24 | if(_nns) 25 | { 26 | delete _nns; 27 | _nns = NULL; 28 | } 29 | _model = model; 30 | _M.resize(_dimension, size); 31 | for(int i=0; i vDist; 46 | vector vIndicesM; 47 | vector vIndicesS; 48 | vector vNonPair; 49 | #pragma omp for schedule(dynamic) 50 | for(int i = 0; i < size; i++) 51 | { 52 | if(mask[i]==1) 53 | { 54 | VectorXf q(_dimension); 55 | for(int j=0; j<_dimension; j++) 56 | q(j) = scene[i][j]; 57 | _nns->knn(q, indices, dists2, 1, 0, NNSearchF::ALLOW_SELF_MATCH); 58 | vIndicesM.push_back((unsigned int)indices(0)); 59 | vIndicesS.push_back(i); 60 | vDist.push_back((double)dists2(0)); 61 | } 62 | else 63 | { 64 | vNonPair.push_back(i); 65 | } 66 | } 67 | 68 | #pragma omp critical 69 | { 70 | for(unsigned int i=0; i 7 | #include 8 | 9 | namespace obvious 10 | { 11 | 12 | /** 13 | * @class LuaScriptManager 14 | * @brief Interface for Lua scription language 15 | * @author Stefan May 16 | * @date 17.6.2015 17 | */ 18 | class LuaScriptManager 19 | { 20 | public: 21 | /** 22 | * Constructor 23 | */ 24 | LuaScriptManager(const char* filepath); 25 | 26 | /** 27 | * Destructor 28 | */ 29 | virtual ~LuaScriptManager(); 30 | 31 | /** 32 | * Reload script 33 | */ 34 | void reload(); 35 | 36 | /** 37 | * Read table from a Lua script 38 | * @param groupname name of group 39 | * @param varnames names of variables in the group 40 | * @param sig signature of parameter list ("b" for bool, "d" for double, "i" for integer, "s" for string), e.g. "bids" = bool integer double char* 41 | * @return success flag 42 | */ 43 | bool readTable(const char* groupname, std::vector &varnames, const char* sig, ...); 44 | 45 | /** 46 | * Call function from a Lua script 47 | * @param func function name 48 | * @param sig signature of parameter list ("d" for double, "i" for integer, "s" for string), e.g. "id>s" = integer double -> return string 49 | * @return success flag 50 | */ 51 | bool callFunction (const char* func, const char* sig, ...); 52 | 53 | /** 54 | * Register C callback function, available in Lua with specified name 55 | * @param func pointer to callback function 56 | * @param name function name available in Lua script 57 | */ 58 | void registerCallback(lua_CFunction func, const char name[]); 59 | 60 | private: 61 | 62 | lua_State* _L; 63 | 64 | std::string _scriptFilePath; 65 | 66 | bool _init; 67 | 68 | }; 69 | 70 | } /* namespace obvious */ 71 | 72 | #endif /* _LUASCRIPTMANAGER_H_ */ 73 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/ProjectivePairAssignment.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECTIVEPAIRASSIGNMENT_H 2 | #define PROJECTIVEPAIRASSIGNMENT_H 3 | 4 | #include "obcore/math/mathbase.h" 5 | #include "obvision/registration/icp/assign/PairAssignment.h" 6 | 7 | 8 | using std::vector; 9 | 10 | namespace obvious 11 | { 12 | 13 | /** 14 | * @class ProjectivePairAssignment 15 | * @brief Encapsulates neighbor searching based on projective projection 16 | * @author Stefan May 17 | **/ 18 | class ProjectivePairAssignment : public PairAssignment 19 | { 20 | public: 21 | 22 | /** 23 | * Standard constructor 24 | * @param P projection matrix 25 | * @param width width of underlying image 26 | * @param height height of underlying image 27 | * @param dimension dimensionality of dataset 28 | **/ 29 | ProjectivePairAssignment(double* P, unsigned int width, unsigned int height, int dimension=3) : PairAssignment(dimension) {init(P, width, height, dimension);}; 30 | 31 | /** 32 | * Standard destructor 33 | **/ 34 | ~ProjectivePairAssignment(); 35 | 36 | /** 37 | * Set model as matching base 38 | * @param model array of xy values 39 | * @param size number of points 40 | **/ 41 | void setModel(double** model, int size); 42 | 43 | /** 44 | * Determine point pairs (nearest neighbors) 45 | * @param scene scene to be compared 46 | * @param size nr of points in scene 47 | * @param pairs return value of neighbors 48 | * @param nonPairs return value of points with no neighbors 49 | */ 50 | void determinePairs(double** scene, bool* msk, int size); 51 | 52 | private: 53 | 54 | /** 55 | * Private initialization routine called by constructors 56 | */ 57 | void init(double* P, unsigned int width, unsigned int height, unsigned int dim); 58 | 59 | double* _P; 60 | unsigned int _w; 61 | unsigned int _h; 62 | unsigned int* _idx_m; 63 | double** _model; 64 | }; 65 | 66 | } 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /applications/xtionStream.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "obgraphic/Obvious3D.h" 3 | #include "obdevice/OpenNiDevice.h" 4 | 5 | using namespace std; 6 | using namespace obvious; 7 | 8 | Obvious3D* _viewer; 9 | VtkCloud* _cloud; 10 | OpenNiDevice* _xtion; 11 | bool _pause = false; 12 | bool _showNormals = false; 13 | 14 | class vtkTimerCallback : public vtkCommand 15 | { 16 | public: 17 | static vtkTimerCallback *New() 18 | { 19 | vtkTimerCallback *cb = new vtkTimerCallback; 20 | return cb; 21 | } 22 | 23 | virtual void Execute(vtkObject *vtkNotUsed(caller), unsigned long eventId, void *vtkNotUsed(callData)) 24 | { 25 | if(!_pause) 26 | { 27 | if(_xtion->grab()) 28 | { 29 | std::vector coords = _xtion->coords(); 30 | double* coordsD = new double[_xtion->width()* _xtion->height() * 3]; 31 | 32 | for(int i=0 ; i<_xtion->width()*_xtion->height()*3 ; i++) 33 | coordsD[i] = coords[i]; 34 | 35 | _cloud->setCoords(coordsD, _xtion->width()*_xtion->height(), 3); 36 | _cloud->setColors(_xtion->rgb(), _xtion->width()*_xtion->height(), 3); 37 | _viewer->update(); 38 | } 39 | } 40 | } 41 | 42 | private: 43 | 44 | }; 45 | 46 | 47 | int main(int argc, char* argv[]) 48 | { 49 | _xtion = new OpenNiDevice(); 50 | _cloud = new VtkCloud(); 51 | _viewer = new Obvious3D("Xtion Stream 3D", 1024, 768, 0, 0); 52 | 53 | _viewer->addCloud(_cloud); 54 | 55 | _xtion->init(); 56 | 57 | vtkSmartPointer cb = vtkSmartPointer::New(); 58 | vtkSmartPointer interactor = _viewer->getWindowInteractor(); 59 | interactor->AddObserver(vtkCommand::TimerEvent, cb); 60 | 61 | interactor->CreateRepeatingTimer(30); 62 | _viewer->startRendering(); 63 | 64 | delete _cloud; 65 | delete _xtion; 66 | 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /obcore/math/IntegratorSimpson.h: -------------------------------------------------------------------------------- 1 | /* 2 | * IntegratorSimpson.h 3 | * 4 | * Created on: 04.09.2013 5 | * Author: chris 6 | */ 7 | 8 | #ifndef INTEGRATORSIMPSON_H_ 9 | #define INTEGRATORSIMPSON_H_ 10 | 11 | /** 12 | * @namespace obvious 13 | */ 14 | namespace obvious 15 | { 16 | /** 17 | * @class IntegratorSimpson 18 | * @date 2013-09-05 19 | * @author Christian Pfitzner 20 | */ 21 | class IntegratorSimpson 22 | { 23 | public: 24 | /** 25 | * Standard constructor 26 | */ 27 | 28 | IntegratorSimpson(void); 29 | /** 30 | * Default destructor 31 | */ 32 | ~IntegratorSimpson(void); 33 | 34 | /** 35 | * Function to set rest time of integrator 36 | * @param T_n rest time in seconds 37 | */ 38 | void setRestTime(const double& T_n); 39 | 40 | /** 41 | * Function to set anti wind up rules 42 | * @param upperLimit upper limit of output 43 | * @param lowerLimit lower limit of output 44 | */ 45 | void setAntiWindup(const double& upperLimit = 10000, 46 | const double& lowerLimit = -10000); 47 | 48 | /** 49 | * Function to update integrator with a new value 50 | * @param y_n new value 51 | * @return output 52 | */ 53 | const double& integrate(const double& error, const double& deltaT); 54 | 55 | /** 56 | * Function to reset integrator 57 | */ 58 | void resetIntegrator(const double& setValue = 0.0); 59 | 60 | private: 61 | double _upperLimit; //!< upper limit of integrator 62 | double _lowerLimit; //!< lower limit of integrator 63 | double _T_n; //!< rest time 64 | 65 | double _e_n, _e_n_1, _e_n_2; //!< error variables 66 | double _y_n, _y_n_1, _y_n_2; //!< output variables 67 | }; 68 | 69 | } // namespace 70 | 71 | 72 | #endif /* INTEGRATORSIMPSON_H_ */ 73 | -------------------------------------------------------------------------------- /obvision/planning/Obstacle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Obstacle.h 3 | * 4 | * Created on: 03.11.2014 5 | * Author: Stefan May 6 | */ 7 | 8 | #ifndef OBSTACLE_H_ 9 | #define OBSTACLE_H_ 10 | 11 | #include 12 | #include "obcore/base/types.h" 13 | 14 | namespace obvious 15 | { 16 | 17 | struct ObstacleBounds 18 | { 19 | obfloat xmin; 20 | obfloat xmax; 21 | obfloat ymin; 22 | obfloat ymax; 23 | }; 24 | 25 | class Obstacle 26 | { 27 | public: 28 | 29 | /** 30 | * Constructor 31 | * @param xcoords x-coordinates of obstacle-related points 32 | * @param ycoords y-coordinates of obstacle-related points 33 | */ 34 | Obstacle(std::vector xcoords, std::vector ycoords); 35 | 36 | /** 37 | * Constructor 38 | * @param bounds bounding box of obstacle 39 | */ 40 | Obstacle(ObstacleBounds bounds); 41 | 42 | /** 43 | * Copy constructor 44 | * @param o obstacle to be copied 45 | */ 46 | Obstacle(Obstacle* o); 47 | 48 | /** 49 | * Get unique obstacle ID 50 | * @return ID 51 | */ 52 | unsigned int getID(); 53 | 54 | /** 55 | * Get bounds of obstacle 56 | * @return bounds 57 | */ 58 | ObstacleBounds* getBounds(); 59 | 60 | /** 61 | * Inflate obstacle 62 | * @param radius used for obstacle inflation 63 | */ 64 | void inflate(obfloat radius); 65 | 66 | /** 67 | * Check if two obstacles intersect with each other 68 | * @param o intersection candidate 69 | * @return intersection flag 70 | */ 71 | bool intersects(Obstacle* o); 72 | 73 | /** 74 | * Fuse obstacles, i.e., merging related regions 75 | * @param xcoords x-coordinates of obstacle 76 | * @param ycoords y-coordinates of obstacle 77 | */ 78 | void merge(std::vector xcoords, std::vector ycoords); 79 | 80 | private: 81 | 82 | ObstacleBounds _bounds; 83 | 84 | unsigned int _id; 85 | }; 86 | 87 | } /* namespace obvious */ 88 | #endif /* OBSTACLE_H_ */ 89 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/FlannPairAssignment.h: -------------------------------------------------------------------------------- 1 | #ifndef FLANNPAIRASSIGNMENT_H 2 | #define FLANNPAIRASSIGNMENT_H 3 | 4 | #include 5 | #include "obcore/math/mathbase.h" 6 | #include "obvision/registration/icp/assign/PairAssignment.h" 7 | 8 | 9 | using std::vector; 10 | 11 | namespace obvious 12 | { 13 | 14 | /** 15 | * @class FlannPairAssignment 16 | * @brief Encapsulates neighbor searching with approximate nearest neighbor algorithm (FLANN) 17 | * @author Stefan May 18 | **/ 19 | class FlannPairAssignment : public PairAssignment 20 | { 21 | public: 22 | 23 | /** 24 | * Standard constructor 25 | * @param dimension dimensionality of dataset 26 | * @param eps used for searching eps-approximate neighbors 27 | **/ 28 | FlannPairAssignment(int dimension, double eps = 0.0, bool parallelSearch=false); 29 | 30 | /** 31 | * Destructor 32 | **/ 33 | ~FlannPairAssignment(); 34 | 35 | /** 36 | * Set model as matching base 37 | * @param model array of xy values 38 | * @param size number of points 39 | **/ 40 | void setModel(double** model, int size); 41 | 42 | /** 43 | * Determine point pairs 44 | * @param scene scene to be compared 45 | * @param msk validity mask 46 | * @param size nr of points in scene 47 | */ 48 | void determinePairs(double** scene, bool* msk, int size); 49 | 50 | private: 51 | 52 | /** 53 | * Private initialization routine called by constructors 54 | */ 55 | void init(double eps); 56 | 57 | /** 58 | * Sequential version of NN-search 59 | */ 60 | void determinePairsSequential(double** scene, bool* mask, int size); 61 | 62 | /** 63 | * OpenMP-accelerated version of NN-search 64 | */ 65 | void determinePairsParallel(double** scene, bool* mask, int size); 66 | 67 | flann::Matrix* _dataset; 68 | flann::Index >* _index; 69 | 70 | double _eps; 71 | 72 | bool _useParallelVersion; 73 | }; 74 | 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /obvision/reconstruct/space/SensorProjective3D.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSOR_PROJECTIVE_3D_H 2 | #define SENSOR_PROJECTIVE_3D_H 3 | 4 | #include "obvision/reconstruct/Sensor.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class SensorProjective3D 11 | * @brief Generic class for 3D measurement units using a pinhole camera model 12 | * @author Stefan May 13 | */ 14 | class SensorProjective3D : public Sensor 15 | { 16 | public: 17 | 18 | /** 19 | * Constructor 20 | * @param[in] cols number of image columns 21 | * @param[in] rows number of image rows 22 | * @param[in] PData 3x4 projection matrix 23 | * @param[in] maxRange maximum range 24 | * @param[in] minRange minimum range 25 | */ 26 | SensorProjective3D(unsigned int cols, unsigned int rows, double PData[12], double maxRange=INFINITY, double minRange=0.0, double lowReflectivityRange=INFINITY); 27 | 28 | /** 29 | * Copy constructor 30 | * @param sensor sensor instance to be copied 31 | */ 32 | SensorProjective3D(SensorProjective3D* sensor); 33 | 34 | /** 35 | * Destructor 36 | */ 37 | ~SensorProjective3D(); 38 | 39 | /** 40 | * Project point to 3D space at certain distance at given pixel 41 | * @param[in] col column index 42 | * @param[in] row row index 43 | * @param[in] depth distance from pixel 44 | * @param[out] coord 3D coordinates (homogeneous) of projected point/direction vector 45 | */ 46 | void project2Space(const unsigned int col, const unsigned int row, const double depth, Matrix* coord); 47 | 48 | /** 49 | * Parallel version of back projection 50 | * @param[in] M matrix of homogeneous 3D coordinates 51 | * @param[out] indices vector of beam indices 52 | * @param[in] T temporary transformation matrix of coordinates 53 | */ 54 | void backProject(Matrix* M, int* indices, Matrix* T=NULL); 55 | 56 | private: 57 | 58 | void init(unsigned int cols, unsigned int rows, double PData[12]); 59 | 60 | Matrix* _P; 61 | 62 | }; 63 | 64 | } 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /obcore/filter/EuclideanFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * EuclideanFilter.cpp 3 | * 4 | * Created on: 29.11.2012 5 | * Author: phil 6 | */ 7 | 8 | #include "obcore/filter/EuclideanFilter.h" 9 | #include "obcore/math/mathbase.h" 10 | 11 | using namespace obvious; 12 | 13 | FILRETVAL EuclideanFilter::applyFilter(void) 14 | { 15 | // check if input and output are set properly 16 | if((!_input)||(!_output)) 17 | { 18 | LOGMSG(DBG_ERROR, "Pointer to input or output invalid"); 19 | return(FILTER_ERROR); 20 | } 21 | double depthVar = 0; 22 | double *dPtr = _input; 23 | _validSize = 0; 24 | 25 | // init output with zeros 26 | for(unsigned int i=0 ; i<_size ; i++) 27 | _output[i]=0.0; 28 | 29 | if (_direction == FILTER_BIGGER) 30 | { 31 | for(unsigned int i=0 ; i<_size ; i+=Point3D::sizeP) { 32 | // new coords array for filtering with centroid 33 | double coord[3] = { dPtr[i] + _centroid[X], 34 | dPtr[i+1] + _centroid[Y], 35 | dPtr[i+2] + _centroid[Z] 36 | }; 37 | depthVar = l2Norm(coord, Point3D::sizeP); 38 | if(depthVar > _threshold) { 39 | dPtr += Point3D::sizeP; 40 | } 41 | else { 42 | for(unsigned int j=0 ; j((double*)dPtr, Point3D::sizeP); 52 | if(depthVar <= _threshold) { 53 | dPtr += Point3D::sizeP; 54 | } 55 | else { 56 | for(unsigned int j=0 ; j 5 | #include "obcore/math/linalg/linalg.h" 6 | #include "obvision/reconstruct/grid/TsdGrid.h" 7 | #include "obvision/reconstruct/grid/SensorPolar2D.h" 8 | 9 | namespace obvious 10 | { 11 | 12 | /** 13 | * @class RayCastPolar2D 14 | * @brief 15 | * @author Stefan May 16 | */ 17 | class RayCastPolar2D 18 | { 19 | public: 20 | 21 | /** 22 | * Constructor 23 | */ 24 | RayCastPolar2D(); 25 | 26 | /** 27 | * Destructor 28 | */ 29 | ~RayCastPolar2D(); 30 | 31 | /** 32 | * Perform raycasting, i.e. calculate coordinates obtainable from the sensor's field of vision 33 | * @param grid grid instance 34 | * @param sensor sensor instance 35 | * @param coords coordinates of intersection between beam and surface 36 | * @param normals normals of surfaces 37 | * @param ctr number of points 38 | */ 39 | void calcCoordsFromCurrentView(TsdGrid* grid, SensorPolar2D* sensor, double* coords, double* normals, unsigned int* ctr); 40 | 41 | /** 42 | * Perform raycasting, i.e. calculate coordinates obtainable from the sensor's field of vision 43 | * @param grid grid instance 44 | * @param sensor sensor instance 45 | * @param coords coordinates of intersection between beam and surface 46 | * @param normals normals of surfaces 47 | * @param mask validity mask 48 | * @return number of valid points, i.e., having mask[i]==true 49 | */ 50 | unsigned int calcCoordsFromCurrentViewMask(TsdGrid* grid, SensorPolar2D* sensor, double* coords, double* normals, bool* mask); 51 | 52 | private: 53 | 54 | bool rayCastFromCurrentView(TsdGrid* grid, obfloat tr[2], obfloat ray[2], obfloat coordinates[2], obfloat normal[2]); 55 | 56 | void calcRayFromCurrentView(const unsigned int beam, double dirVec[2]); 57 | 58 | double _xmin; 59 | double _ymin; 60 | 61 | double _xmax; 62 | double _ymax; 63 | 64 | obfloat _idxMin; 65 | obfloat _idxMax; 66 | }; 67 | 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/AnnPairAssignment.h: -------------------------------------------------------------------------------- 1 | #ifndef ANNPAIRASSIGNMENT_H 2 | #define ANNPAIRASSIGNMENT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "obcore/math/mathbase.h" 8 | #include "obvision/registration/icp/assign/PairAssignment.h" 9 | 10 | 11 | using std::vector; 12 | 13 | namespace obvious 14 | { 15 | 16 | /** 17 | * @class AnnPairAssignment 18 | * @brief Encapsulates neighbor searching with approximate nearest neighbor algorithm (ANN) 19 | * @author Stefan May 20 | **/ 21 | class AnnPairAssignment : public PairAssignment 22 | { 23 | public: 24 | 25 | /** 26 | * Default constructor 27 | */ 28 | AnnPairAssignment(){init();}; 29 | 30 | /** 31 | * Standard constructor 32 | **/ 33 | AnnPairAssignment(int dimension) : PairAssignment(dimension) {init();}; 34 | 35 | /** 36 | * Destructor 37 | **/ 38 | ~AnnPairAssignment(); 39 | 40 | /** 41 | * Set maximum number of point to be visited until interruption. 42 | * This option enhances the performance, but can raise non-optimal matches. 43 | * @param visitPoints number of points to be visited 44 | */ 45 | void setMaxVisitPoints(unsigned int visitPoints); 46 | 47 | /** 48 | * Set model as matching base 49 | * @param model array of xy values 50 | * @param size number of points 51 | **/ 52 | void setModel(double** model, int size); 53 | 54 | /** 55 | * Determine point pairs 56 | * @param scene scene to be compared 57 | * @param msk validity mask 58 | * @param size nr of points in scene 59 | */ 60 | void determinePairs(double** scene, bool* msk, int size); 61 | 62 | /** 63 | * Get the constructed kd-tree 64 | * @return Pointer to the internally used kd-tree-structure constructed by libANN 65 | */ 66 | ANNkd_tree* getTree(); 67 | 68 | private: 69 | 70 | /** 71 | * Private initialization routine called by constructors 72 | */ 73 | void init(); 74 | 75 | /** 76 | * KD search tree 77 | */ 78 | ANNkd_tree* _tree; 79 | 80 | }; 81 | 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /obcore/statemachine/states/StateLua.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "obcore/statemachine/AgentModel.h" 5 | #include "obcore/base/Logger.h" 6 | #include "StateLua.h" 7 | 8 | namespace obvious 9 | { 10 | 11 | /** 12 | * Callback function for Lua script 13 | */ 14 | static int luaTransistionToPersistantState(lua_State* L) 15 | { 16 | unsigned int agentID = luaL_checknumber(L, 1); 17 | unsigned int stateID = luaL_checknumber(L, 2); 18 | 19 | Agent* agent = Agent::getAgentByID(agentID); 20 | if(agent) 21 | agent->transitionToPersistantState(stateID); 22 | else 23 | LOGMSG(DBG_ERROR, "Agent ID not valid"); 24 | 25 | return 0; 26 | } 27 | 28 | StateLua::StateLua(AgentModel* model, const char* filepath) : StateBaseModel(model) 29 | { 30 | _manager = new LuaScriptManager(filepath); 31 | _manager->registerCallback(luaTransistionToPersistantState, "transitionToPersistantState"); 32 | _autoReload = true; 33 | } 34 | 35 | StateLua::~StateLua() 36 | { 37 | delete _manager; 38 | } 39 | 40 | void StateLua::setAutoReload(bool autoReload) 41 | { 42 | _autoReload = autoReload; 43 | } 44 | 45 | bool StateLua::getAutoReload() 46 | { 47 | return _autoReload; 48 | } 49 | 50 | void StateLua::onSetup() 51 | { 52 | unsigned int agentID = _agent->getID(); 53 | _manager->callFunction("onLoad", "i", agentID); 54 | _loadInThisCycle = false; 55 | } 56 | 57 | void StateLua::onEntry() 58 | { 59 | if(_loadInThisCycle) reload(); 60 | 61 | _manager->callFunction("onEntry", ""); 62 | } 63 | 64 | void StateLua::onActive() 65 | { 66 | if(_loadInThisCycle) reload(); 67 | 68 | int retval = 0; 69 | _manager->callFunction("onActive", ">i", &retval); 70 | 71 | _loadInThisCycle = _autoReload; 72 | } 73 | 74 | void StateLua::onExit() 75 | { 76 | _manager->callFunction("onExit", ""); 77 | } 78 | 79 | void StateLua::reload() 80 | { 81 | _manager->reload(); 82 | unsigned int agentID = _agent->getID(); 83 | _manager->callFunction("onLoad", "i", agentID); 84 | _loadInThisCycle = false; 85 | } 86 | 87 | } /* namespace obvious */ 88 | -------------------------------------------------------------------------------- /obdevice/CloudFactory.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * 3 | * Copyright (C) 2014 by TH-Nürnberg 4 | * Written by Christian Merkl 5 | * All Rights Reserved. 6 | * 7 | * This program is free software; you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation; either version 2 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License along 18 | * with this program; if not, write to the Free Software Foundation, Inc., 19 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 20 | * 21 | *********************************************************************************/ 22 | #ifndef ___CLOUD_FACTORY_H___ 23 | #define ___CLOUD_FACTORY_H___ 24 | 25 | #include "obcore/base/PointCloud.h" 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | 33 | namespace obvious { 34 | 35 | class CloudFactory 36 | { 37 | public: 38 | static void generateRandomCloud(PointCloud& cloud, const std::size_t size); 39 | static void generateRandomCloud(PointCloud& cloud, const std::size_t size); 40 | static bool loadCloud(PointCloud& cloud, const std::string& file); 41 | static bool loadCloud(PointCloud& cloud, const std::string& file); 42 | // static bool saveCloud(const PointCloud& cloud, const std::string& file); 43 | 44 | private: 45 | static void readLineAndSplit(std::ifstream& stream, std::vector& tokens); 46 | static bool dropLines(std::ifstream& stream, const unsigned int lines = 1); 47 | }; 48 | 49 | } // end namespace obvious 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /obvision/registration/icp/IRigidEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef IRIGIDESTIMATOR_H_ 2 | #define IRIGIDESTIMATOR_H_ 3 | 4 | #include 5 | 6 | #include "obvision/registration/icp/assign/assignbase.h" 7 | #include "obcore/math/linalg/linalg.h" 8 | #include 9 | 10 | namespace obvious 11 | { 12 | 13 | /** 14 | * @class IRigidEstimator 15 | * @brief An estimator for registering points clouds into one common coordinate system. 16 | * @author Stefan May 17 | */ 18 | class IRigidEstimator 19 | { 20 | public: 21 | /** 22 | * Default constructor 23 | */ 24 | IRigidEstimator(){}; 25 | 26 | /** 27 | * Destructor 28 | */ 29 | virtual ~IRigidEstimator(){}; 30 | 31 | /** 32 | * Setting internal pointer to model array. The model is seen as the ground truth against which the scene has to be registered. 33 | * @param model Pointer to 3 dimensional model array 34 | */ 35 | virtual void setModel(double** model, unsigned int size, double** normals=NULL) = 0; 36 | 37 | /** 38 | * Setting internal pointer to scene array. (See commend for setModel) 39 | * @param scene Pointer to 3 dimensional scene array 40 | */ 41 | virtual void setScene(double** scene, unsigned int size, double** normals=NULL) = 0; 42 | 43 | /** 44 | * Setting assigned point pairs. You can use a pair assigner for this purpose. The deviation, i.e. the mean distance, between those pairs is also determined within this method. 45 | * @param vPairs Vector of pairs of indices. Each index pair references a scene and a model point. 46 | */ 47 | virtual void setPairs(std::vector* vPairs) = 0; 48 | 49 | /** 50 | * Access the RMS error that has been calculated by the setPairs method. 51 | * @return RMS error 52 | */ 53 | virtual double getRMS() = 0; 54 | 55 | /** 56 | * Determine the transformation matrix that registers the scene to the model. 57 | * @param transformation matrix as return parameter 58 | */ 59 | virtual void estimateTransformation(Matrix* T) = 0; 60 | }; 61 | 62 | } 63 | 64 | #endif /*IRIGIDESTIMATOR_H_*/ 65 | -------------------------------------------------------------------------------- /obdevice/SickLMS100.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SickLMS100.h 3 | * 4 | * Created on: 11.03.2013 5 | * Author: christian 6 | */ 7 | 8 | 9 | 10 | #ifndef SICKLMS100_H_ 11 | #define SICKLMS100_H_ 12 | 13 | #include 14 | 15 | /** 16 | * @namespace obvious 17 | */ 18 | namespace obvious 19 | { 20 | /** 21 | * @class LaserDevice 22 | */ 23 | class SickLMS100 24 | { 25 | public: 26 | /** 27 | * Standard Constructor 28 | */ 29 | SickLMS100(); 30 | 31 | /** 32 | * Default Destructor 33 | */ 34 | virtual ~SickLMS100(); 35 | 36 | double getStartAngle(); 37 | 38 | double getStopAngle(); 39 | 40 | unsigned int getNumberOfRays(); 41 | 42 | double getAngularRes(void); 43 | 44 | double* getRanges(); 45 | 46 | double* getCoords(); 47 | 48 | /** 49 | * Function to grab new data 50 | * @return TRUE if success 51 | */ 52 | virtual bool grab(void); 53 | 54 | void schedule(); 55 | 56 | private: 57 | 58 | /** 59 | * Function to estimate ranges in scan 60 | */ 61 | void calculateRanges(void); 62 | 63 | /** 64 | * Function to estimate intensities in scan 65 | */ 66 | void calculateIntensities(void); 67 | 68 | /** 69 | * Function to estimate single angles for every ray 70 | */ 71 | void calculateAngles(void); 72 | 73 | /** 74 | * Function to estimate 2D coords 75 | */ 76 | void calculateCoords2D(void); 77 | 78 | LMS1xx _laser; 79 | scanCfg _cfg; 80 | scanDataCfg _dataCfg; 81 | scanData _data; 82 | scanData _dataBuffer; 83 | unsigned int _nrOfRays; 84 | 85 | double* _ranges; //!< Distance in meters 86 | double* _intensities; //!< Intensities 87 | double* _coords2D; //!< 2D coords 88 | double* _normals; //!< normals 89 | double* _angles; //!< Angles in rad 90 | bool* _mask; //!< mask for valid or invalid points 91 | 92 | }; 93 | 94 | }; //namespace 95 | 96 | 97 | #endif /* SICKLMS100_H_ */ 98 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/RobotFootprintFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RobotFootprintFilter.h 3 | * 4 | * Created on: 20.09.2013 5 | * Author: chris 6 | */ 7 | 8 | #ifndef ROBOTFOOTPRINTFILTER_H_ 9 | #define ROBOTFOOTPRINTFILTER_H_ 10 | 11 | #include "obvision/registration/icp/assign/filter/IPreAssignmentFilter.h" 12 | #include "obcore/math/mathbase.h" 13 | 14 | namespace obvious 15 | { 16 | 17 | class RobotFootprintFilter : public IPreAssignmentFilter 18 | { 19 | public: 20 | /** 21 | * Default constructor with radius mode 22 | * @param maxRadius 23 | */ 24 | RobotFootprintFilter(unsigned int dim, double maxRadius); 25 | 26 | /** 27 | * Constructor to initialize filter with a box 28 | * @param[in] x_lim min and max value for x axis 29 | * @param[in] y_lim min and max value for y axis 30 | * @param[in] z_lim min and max value for z axis 31 | */ 32 | /*RobotFootprintFilter(const double* x_lim, 33 | const double* y_lim, 34 | const double* z_lim);*/ 35 | /** 36 | * Default destructor 37 | */ 38 | ~RobotFootprintFilter(); 39 | /** 40 | * Function to set offset 41 | * @param[in] offset offset in R3 coordinates 42 | */ 43 | //void setOffset(const double* offset); 44 | /** 45 | * Function to set min and max radius 46 | * @param[in] minRadius minimal radius around offset 47 | * @param[in] maxRadius maximum radius around offset 48 | */ 49 | //void setRadius(const double minRadius, 50 | // const double maxRadius); 51 | /** 52 | * Virtual function of IPreAssignmentFilter 53 | * @param scene 54 | * @param size 55 | * @param mask 56 | */ 57 | virtual void filter(double** scene, unsigned int size, bool* mask); 58 | 59 | private: 60 | 61 | unsigned int _dim; 62 | //double* _offset; 63 | //double _minRadius; 64 | double _maxRadius; 65 | //double* _x_lim; 66 | //double* _y_lim; 67 | //double* _z_lim; 68 | 69 | 70 | }; 71 | 72 | }; 73 | 74 | #endif /* ROBOTFOOTPRINTFILTER_H_ */ 75 | -------------------------------------------------------------------------------- /obcore/filter/BoundingBoxFilter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FilterBoundingBox.h 3 | * @author Christian Pfitzner 4 | * @date 10.12.2012 5 | * 6 | * 7 | */ 8 | 9 | #ifndef FILTERBOUNDINGBOX_H_ 10 | #define FILTERBOUNDINGBOX_H_ 11 | 12 | #include "obcore/filter/FilterDistance.h" 13 | #include "obcore/filter/CartesianFilter.h" 14 | 15 | /** 16 | * @namespace obvious 17 | */ 18 | namespace obvious 19 | { 20 | /** 21 | * @class 22 | */ 23 | class BoundingBoxFilter : public FilterDistance 24 | { 25 | public: 26 | /** 27 | * @struct Dimension 28 | */ 29 | struct Dimension{ 30 | double x; 31 | double y; 32 | double z; 33 | }; 34 | /** 35 | * Standard constructor with initialization 36 | */ 37 | BoundingBoxFilter(void) 38 | : FilterDistance() 39 | { 40 | _pFilter = new CartesianFilter; 41 | _dim.x = 0.0; 42 | _dim.y = 0.0; 43 | _dim.z = 0.0; 44 | } 45 | /** 46 | * Standard destructor 47 | */ 48 | ~BoundingBoxFilter(); 49 | /** 50 | * Function to start filtering 51 | * 52 | * @note This function must be called after setting input and output of 53 | * base class. 54 | */ 55 | FILRETVAL applyFilter(void); 56 | /** 57 | * Function to get set centroid 58 | * @return _centroid 59 | */ 60 | void setCentroid(const Point3D& center); 61 | /** 62 | * Function to set up dimension of bounding box around centroid 63 | * @param x x value 64 | * @param y y value 65 | * @param z z value 66 | */ 67 | void setDimension(double x, double y, double z); 68 | /** 69 | * Function to set up dimension of dice for filtering 70 | * @param x edge length of dice 71 | */ 72 | void setDimension(double val); 73 | private: 74 | CartesianFilter* _pFilter; //!< object of cartesian filter 75 | Dimension _dim; //!< structure for dimension of box 76 | 77 | // functions private to avoid public implementation 78 | void setThreshold(const double& val) { } 79 | void setFilterDirection(Direction direction) { } 80 | }; 81 | 82 | }; 83 | 84 | 85 | 86 | #endif /* FILTERBOUNDINGBOX_H_ */ 87 | -------------------------------------------------------------------------------- /obdevice/UvcVirtualCam.h: -------------------------------------------------------------------------------- 1 | #ifndef UVCVIRTUALCAM_H 2 | #define UVCVIRTUALCAM_H 3 | 4 | #include "obdevice/UvcCam.h" 5 | 6 | using namespace std; 7 | 8 | /** 9 | * @namespace obvious 10 | */ 11 | namespace obvious 12 | { 13 | 14 | /** 15 | * @class UvcVirtualCam 16 | * @brief Class encapsulates virtual UVC camera device with subsampled resolution 17 | * @author Stefan May, Philipp Koch 18 | **/ 19 | class UvcVirtualCam 20 | { 21 | 22 | public: 23 | 24 | /** 25 | * Standard constructor 26 | */ 27 | UvcVirtualCam(const char* dev, unsigned int maxWidth, unsigned int maxHeight, EnumCameraColorMode mode); 28 | 29 | /** 30 | * Standard destructor 31 | */ 32 | ~UvcVirtualCam(); 33 | 34 | /** 35 | * Get width of virtual camera image 36 | * @return width 37 | */ 38 | unsigned int getWidth(); 39 | 40 | /** 41 | * Get height of virtual camera image 42 | * @return height 43 | */ 44 | unsigned int getHeight(); 45 | 46 | /** 47 | * Set scale factor (image must be a multiple in each dimension) 48 | * @param scale downsampling factor in each dimension 49 | * @return success 50 | */ 51 | EnumCameraError setScale(unsigned int scale); 52 | 53 | /** 54 | * Set color mode of returned images 55 | * @param mode color mode 56 | */ 57 | void setColorMode(EnumCameraColorMode mode); 58 | 59 | /** 60 | * Get number of image channels, i.e., RGB=3, Grayscale=1 61 | * @return image channels 62 | */ 63 | unsigned int getChannels(); 64 | 65 | /** 66 | * Grab images from device in non-blocking mode. 67 | * @param img Pointer to image buffer (must be instantiated externally) 68 | * @return Grabbing state 69 | */ 70 | EnumCameraError grab(unsigned char* img); 71 | 72 | private: 73 | 74 | void average(unsigned char* img, unsigned int cols, unsigned int rows, unsigned int scale); 75 | 76 | UvcCam* _cam; 77 | unsigned char* _buf; 78 | unsigned char* _bufR; 79 | unsigned char* _bufG; 80 | unsigned char* _bufB; 81 | unsigned int* _bufI; 82 | 83 | unsigned int _maxWidth; 84 | unsigned int _maxHeight; 85 | unsigned int _scale; 86 | }; 87 | 88 | } 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /obcore/base/Time.cpp: -------------------------------------------------------------------------------- 1 | #include "Time.h" 2 | 3 | #ifdef WIN32 4 | #include 5 | #else 6 | #include 7 | #include 8 | #endif 9 | 10 | namespace obvious { 11 | 12 | #ifdef WIN32 13 | LONG64 Time::_ticksPerSecond = 0; 14 | #endif 15 | 16 | Time::Time(void) 17 | { 18 | #ifdef WIN32 19 | if(!_ticksPerSecond) QueryPerformanceFrequency((LARGE_INTEGER*)&_ticksPerSecond); 20 | #endif 21 | 22 | _seconds = 0; 23 | _mus = 0; 24 | } 25 | 26 | Time Time::now(void) 27 | { 28 | Time time; 29 | 30 | #ifdef WIN32 31 | LARGE_INTEGER tick; 32 | QueryPerformanceCounter(&tick); 33 | tick.QuadPart = tick.QuadPart * 1000 / _ticksPerSecond; 34 | time._seconds = tick.u.LowPart; 35 | time._mus = tick.u.LowPart * 1e3 - time._seconds * 1e6; 36 | #else 37 | timeval clock; 38 | ::gettimeofday(&clock, 0); 39 | time._seconds = clock.tv_sec; 40 | time._mus = clock.tv_usec; 41 | #endif 42 | 43 | return time; 44 | } 45 | 46 | double Time::hours(void) const 47 | { 48 | return static_cast(_seconds) / 3600.0; 49 | } 50 | 51 | double Time::min(void) const 52 | { 53 | return static_cast(_seconds) / 60.0; 54 | } 55 | 56 | double Time::sec(void) const 57 | { 58 | return static_cast(_seconds) + static_cast(_mus) * 1.0e-6; 59 | } 60 | 61 | double Time::mus(void) const 62 | { 63 | return static_cast(_seconds) * 1.0e6 + static_cast(_mus); 64 | } 65 | 66 | Time& Time::operator=(const Time& time) 67 | { 68 | _seconds = time._seconds; 69 | _mus = time._mus; 70 | return *this; 71 | } 72 | 73 | double Time::operator-(const Time& time) const 74 | { 75 | return this->sec() - time.sec(); 76 | } 77 | 78 | Time& Time::operator+=(const double sec) 79 | { 80 | _seconds += static_cast(sec); 81 | _mus += std::fmod(1.0f, sec) * 1.0e-6; 82 | return *this; 83 | } 84 | 85 | std::ostream& operator<<(std::ostream& out, const Time& time) 86 | { 87 | out << "[" << time._seconds / 3600 << ":" 88 | << time._seconds % 3600 << ":" 89 | << time._seconds % 60 << "]"; 90 | 91 | return out; 92 | } 93 | 94 | } // end namespace obvious 95 | -------------------------------------------------------------------------------- /applications/uvcvirtualcam_serialize.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "obdevice/UvcVirtualCam.h" 3 | #include "obcore/base/tools.h" 4 | 5 | using namespace std; 6 | using namespace obvious; 7 | 8 | int main(int argc, char* argv[]) 9 | { 10 | unsigned int width = 640; 11 | unsigned int height = 480; 12 | unsigned int scale = 1; 13 | EnumCameraColorMode mode = CAMRGB; 14 | 15 | char* dev = (char*)"/dev/video0"; 16 | 17 | if(argc>1) dev = argv[1]; 18 | 19 | if(argc>2) 20 | { 21 | if(argc!=6) 22 | { 23 | cout << "usage: " << argv[0] << " [device] [width height scale colormode]" << endl; 24 | cout << " colormode: 1=Grayscale, else=RGB" << endl; 25 | return 0; 26 | } 27 | width = atoi(argv[2]); 28 | height = atoi(argv[3]); 29 | scale = atoi(argv[4]); 30 | int tmp = atoi(argv[5]); 31 | if(tmp==1) mode = CAMGRAYSCALE; 32 | } 33 | 34 | if(scale<1) 35 | { 36 | cout << "Scale factor must be greater than 0." << endl; 37 | return -1; 38 | } 39 | 40 | UvcVirtualCam* cam = new UvcVirtualCam(dev, width, height, mode); 41 | EnumCameraError retval = cam->setScale(scale); 42 | 43 | if(retval!=CAMSUCCESS) return -1; 44 | 45 | unsigned char* img = new unsigned char[cam->getWidth() * cam->getHeight() * 3]; 46 | 47 | retval = cam->grab(img); 48 | 49 | if(retval==CAMSUCCESS) 50 | { 51 | if(mode==CAMRGB) 52 | { 53 | char* path = (char*)"/tmp/test.ppm"; 54 | cout << "Serializing image to " << path << " (width: " << cam->getWidth() << ", " << cam->getHeight() << ")" << endl; 55 | serializePPM(path, img, cam->getWidth(), cam->getHeight(), 0); 56 | } 57 | else 58 | { 59 | char* path = (char*)"/tmp/test.pgm"; 60 | cout << "Serializing image to " << path << " (width: " << cam->getWidth() << ", " << cam->getHeight() << ")" << endl; 61 | serializePGM(path, img, cam->getWidth(), cam->getHeight(), 0); 62 | } 63 | } 64 | else 65 | { 66 | cout << "Failed to connect to camera " << dev << endl; 67 | } 68 | 69 | delete [] img; 70 | delete cam; 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /obvision/reconstruct/grid/TsdGridComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef TSDGRIDCOMPONENT_H 2 | #define TSDGRIDCOMPONENT_H 3 | 4 | #include "obcore/base/types.h" 5 | #include "obcore/math/linalg/linalg.h" 6 | #include "obvision/reconstruct/reconstruct_defs.h" 7 | #include "obvision/reconstruct/Sensor.h" 8 | 9 | namespace obvious 10 | { 11 | 12 | /** 13 | * @class TsdGridComponent 14 | * @brief Abstract component for quadtree implementation 15 | * @author Stefan May 16 | */ 17 | class TsdGridComponent 18 | { 19 | public: 20 | /** 21 | * Constructor 22 | * @brief Abstraction layer for leafs and branches 23 | * @param isLeaf discriminate leaf from branch 24 | */ 25 | TsdGridComponent(bool isLeaf); 26 | 27 | /** 28 | * Destructor 29 | */ 30 | virtual ~TsdGridComponent(); 31 | 32 | /** 33 | * Edge length of component 34 | * @return edge length 35 | */ 36 | obfloat getComponentSize(); 37 | 38 | /** 39 | * Get coordinates of centroid 40 | * @return coordinate array 41 | */ 42 | obfloat* getCentroid(); 43 | 44 | /** 45 | * Get circumradius, i.e., diagonal length of component 46 | * @return circumradius 47 | */ 48 | obfloat getCircumradius(); 49 | 50 | /** 51 | * Get homogeneous coordinates of edges 52 | * @return coordinate array 53 | */ 54 | Matrix* getEdgeCoordsHom(); 55 | 56 | /** 57 | * Discriminate leaf from branch 58 | * @return discrimination flag 59 | */ 60 | bool isLeaf(); 61 | 62 | /** 63 | * Check whether component is in range from a certain sensor position 64 | * @param pos sensor position 65 | * @param sensor sensor 66 | * @param maxTruncation maximum truncation radius 67 | * @return visibility flag 68 | */ 69 | bool isInRange(obfloat pos[2], Sensor* sensor, obfloat maxTruncation); 70 | 71 | /** 72 | * Increase emptiness of component (for visible cells, in which every measurement ray passes through without hitting a surface) 73 | */ 74 | virtual void increaseEmptiness() = 0; 75 | 76 | protected: 77 | 78 | obfloat _componentSize; 79 | 80 | bool _isLeaf; 81 | 82 | obfloat _centroid[2]; 83 | 84 | Matrix* _edgeCoordsHom; 85 | 86 | obfloat _circumradius; 87 | 88 | }; 89 | 90 | } 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /test/obcore/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project(math_test) 3 | 4 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp -Wall -O2 -DNDEBUG -pipe -march=native") 5 | 6 | # Setup testing 7 | add_subdirectory(./gtest-1.7.0 ./gtest-1.7.0/bin) 8 | enable_testing() 9 | include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR} $ENV{OBVIOUSLY_ROOT}) 10 | link_directories($ENV{OBVIOUSLY_ROOT}/build/release/obcore 11 | $ENV{OBVIOUSLY_ROOT}/build/release/obdevice 12 | $ENV{OBVIOUSLY_ROOT}/build/release/obgraphic 13 | ) 14 | 15 | # experimental 16 | #find_package(Eigen REQUIRED) 17 | find_package(Qt4 REQUIRED) 18 | find_package(VTK REQUIRED) 19 | 20 | include(${QT_USE_FILE}) 21 | include(${VTK_USE_FILE}) 22 | 23 | add_definitions(${QT_DEFINITIONS}) 24 | 25 | include_directories(${Eigen_INCLUDE_DIRS}) 26 | 27 | 28 | 29 | # Add test cpp file 30 | add_executable(runMatrixTest 31 | math/MatrixTest.cpp 32 | ) 33 | 34 | add_executable(runQuaternionTest 35 | math/QuaternionTest.cpp 36 | ) 37 | 38 | #add_executable(eigen-vs-gsl 39 | # base/eigen-vs-gsl.cpp 40 | # ) 41 | 42 | #add_executable(pointcloud 43 | # base/pointcloud.cpp 44 | # ) 45 | 46 | 47 | # Link test executable against gtest & gtest_main 48 | target_link_libraries(runMatrixTest gtest gtest_main obcore gsl gslcblas) 49 | 50 | target_link_libraries(runQuaternionTest gtest gtest_main obcore gsl gslcblas) 51 | 52 | #target_link_libraries(eigen-vs-gsl 53 | # obcore 54 | # gsl 55 | # gslcblas 56 | # ) 57 | 58 | #target_link_libraries(pointcloud 59 | # obcore 60 | # obdevice 61 | # obgraphic 62 | # ${QT_LIBRARIES} 63 | # QVTK 64 | # ${VTK_LIBRARIES} 65 | # gsl 66 | # gslcblas 67 | # ) 68 | 69 | add_test( 70 | NAME runMatrixTest 71 | COMMAND runMatrixTest 72 | ) 73 | 74 | add_test( 75 | NAME runQuaternionTest 76 | COMMAND runQuaternionTest 77 | ) 78 | -------------------------------------------------------------------------------- /applications/nanoStream.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "obgraphic/Obvious3D.h" 3 | //#include "obgraphic/Obvious2D.h" 4 | #include "obdevice/CamNano.h" 5 | 6 | using namespace std; 7 | using namespace obvious; 8 | 9 | Obvious3D* _viewer; 10 | //Obvious2D* _viewer2D; 11 | VtkCloud* _cloud; 12 | CamNano* _nano; 13 | bool _pause = false; 14 | bool _showNormals = false; 15 | 16 | class vtkTimerCallback : public vtkCommand 17 | { 18 | public: 19 | static vtkTimerCallback *New() 20 | { 21 | vtkTimerCallback *cb = new vtkTimerCallback; 22 | return cb; 23 | } 24 | 25 | virtual void Execute(vtkObject *vtkNotUsed(caller), unsigned long eventId, void *vtkNotUsed(callData)) 26 | { 27 | if(!_pause) 28 | { 29 | if(_nano->grab()) 30 | { 31 | std::cout << "Frame rate: \t\t" << _nano->getFrameRate() << std::endl; 32 | _cloud->setCoords(_nano->getValidCoords(), _nano->getValidSize(), 3); 33 | // std::cout << "valid size: " << _nano->getValidSize() << std::endl; 34 | // _cloud->setColors(_nano->getRGB(), _nano->getCols()*_nano->getRows(), 3); 35 | _viewer->update(); 36 | } 37 | } 38 | } 39 | 40 | private: 41 | 42 | }; 43 | 44 | 45 | int main(int argc, char* argv[]) 46 | { 47 | _nano = new CamNano(); 48 | _cloud = new VtkCloud(); 49 | _viewer = new Obvious3D("Nano Stream 3D", 1024, 768, 0, 0); 50 | // _viewer2D = new Obvious2D(1024, 768, "Depth Image"); 51 | 52 | _nano->showParameters(); 53 | // unsigned char* _img = new unsigned char[_nano->getCols()*_nano->getRows()*3]; 54 | // while(_viewer2D->isAlive()) 55 | // { 56 | // _img = _nano->getImage(); 57 | // _viewer2D->draw(_img, _nano->getCols(), _nano->getRows(), 3); 58 | // } 59 | 60 | 61 | // _nano->setIntegrationTime(120); 62 | _viewer->addCloud(_cloud); 63 | 64 | vtkSmartPointer cb = vtkSmartPointer::New(); 65 | vtkSmartPointer interactor = _viewer->getWindowInteractor(); 66 | interactor->AddObserver(vtkCommand::TimerEvent, cb); 67 | 68 | interactor->CreateRepeatingTimer(1); 69 | _viewer->startRendering(); 70 | 71 | // delete _img; 72 | delete _cloud; 73 | delete _nano; 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /obvision/planning/AStarNode.cpp: -------------------------------------------------------------------------------- 1 | #include "AStarNode.h" 2 | 3 | #include "obvision/planning/AStarNode.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace obvious 11 | { 12 | 13 | 14 | AStarNode::AStarNode(int xp, int yp, int d, int p, int currentDir) 15 | { 16 | _xPos = xp; 17 | _yPos = yp; 18 | _level = d; 19 | _priority = p; 20 | _currentDir = currentDir; 21 | _isOverwritten = false; 22 | } 23 | 24 | AStarNode::~AStarNode() 25 | { 26 | 27 | } 28 | 29 | void AStarNode::setCurrentDir(int dir) 30 | { 31 | _currentDir = dir; 32 | } 33 | 34 | int AStarNode::getCurrentDir() const 35 | { 36 | return _currentDir; 37 | } 38 | 39 | int AStarNode::getxPos() const 40 | { 41 | return _xPos; 42 | } 43 | 44 | int AStarNode::getyPos() const 45 | { 46 | return _yPos; 47 | } 48 | 49 | int AStarNode::getLevel() const 50 | { 51 | return _level; 52 | } 53 | 54 | int AStarNode::getPriority() const 55 | { 56 | return _priority; 57 | } 58 | 59 | void AStarNode::updatePriority(const int & xDest, const int & yDest) 60 | { 61 | // factor 10 is used to represent distance with two digits precision 62 | _priority=_level+estimate(xDest, yDest)*10; 63 | } 64 | 65 | void AStarNode::nextLevelPenalty(const int & i) 66 | { 67 | // give better priority to going strait instead of diagonally 68 | //_level+=(i%2==0?10:14); 69 | 70 | // factor 10 is used to represent distance with two digits precision 71 | // 14 represents diagonal 72 | // a penalty is added when the direction needs to be changed 73 | _level+=((i%2==0?10:14) + (( ((i+4)%8)==_currentDir) ? 0 : 1)); 74 | } 75 | 76 | void AStarNode::nextLevel(const int & i) 77 | { 78 | // give better priority to going strait instead of diagonally 79 | //_level+=(i%2==0?10:14); 80 | 81 | // factor 10 is used to represent distance with two digits precision 82 | // 14 represents diagonal 83 | // a penalty is added when the direction needs to be changed 84 | _level+=(i%2==0?10:14); 85 | } 86 | 87 | const int AStarNode::estimate(const int & xDest, const int & yDest) const 88 | { 89 | int xd = xDest-_xPos; 90 | int yd = yDest-_yPos; 91 | 92 | // Euclidian Distance 93 | return(sqrt(xd*xd+yd*yd)); 94 | } 95 | 96 | } /* namespace obvious */ 97 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/TrimmedFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "TrimmedFilter.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace obvious 7 | { 8 | 9 | struct StrObvisionTrimPair 10 | { 11 | unsigned int unIdx; 12 | double dDistanceSqr; 13 | }; 14 | 15 | bool operator < (const StrObvisionTrimPair& first, const StrObvisionTrimPair& second) 16 | { 17 | if (first.dDistanceSqr < second.dDistanceSqr) return true; 18 | return false; 19 | } 20 | 21 | TrimmedFilter::TrimmedFilter(unsigned int unOverlap) 22 | { 23 | _unOverlap = unOverlap; 24 | }; 25 | 26 | TrimmedFilter::~TrimmedFilter() 27 | { 28 | 29 | }; 30 | 31 | void TrimmedFilter::filter(double** model, 32 | double** scene, 33 | vector* pairs, 34 | vector* distancesSqr, 35 | vector* fpairs, 36 | vector* fdistancesSqr, 37 | vector* nonPairs) 38 | { 39 | if(!_active) 40 | { 41 | *fpairs = *pairs; 42 | *fdistancesSqr = *distancesSqr; 43 | return; 44 | } 45 | 46 | unsigned int unPairs = pairs->size(); 47 | unsigned int unConsideredPairs = (unPairs * this->_unOverlap) / 100; 48 | 49 | fpairs->clear(); 50 | fdistancesSqr->clear(); 51 | 52 | vector vTrimmedPairs; 53 | 54 | unsigned int p=0; 55 | for(p=0; p unConsideredPairs ? unConsideredPairs : unPairs); 66 | 67 | for(p=0; ppush_back(pair); 72 | fdistancesSqr->push_back((*distancesSqr)[unIdx]); 73 | } 74 | for(p=unRetVals; ppush_back((*pairs)[unIdx].indexSecond); 78 | } 79 | } 80 | 81 | } 82 | -------------------------------------------------------------------------------- /applications/uvccam_serialize.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "obdevice/UvcCam.h" 3 | #include "obcore/base/tools.h" 4 | 5 | using namespace std; 6 | using namespace obvious; 7 | 8 | int main(int argc, char* argv[]) 9 | { 10 | unsigned int width = 640; 11 | unsigned int height = 480; 12 | EnumCameraColorMode mode = CAMRGB; 13 | 14 | char* dev = (char*)"/dev/video0"; 15 | 16 | if(argc>1) dev = argv[1]; 17 | 18 | if(argc>2) 19 | { 20 | if(argc!=5) 21 | { 22 | cout << "usage: " << argv[0] << " [device] [width height colormode]" << endl; 23 | cout << " colormode: 1=Grayscale, else=RGB" << endl; 24 | return 0; 25 | } 26 | width = atoi(argv[2]); 27 | height = atoi(argv[3]); 28 | int tmp = atoi(argv[4]); 29 | if(tmp==1) mode = CAMGRAYSCALE; 30 | } 31 | 32 | UvcCam* cam = new UvcCam(dev, width, height); 33 | unsigned char* img = new unsigned char[width*height*3]; 34 | 35 | EnumCameraError retval = cam->connect(); 36 | if(retval != CAMSUCCESS) return -1; 37 | 38 | unsigned int format = V4L2_PIX_FMT_YUYV; 39 | retval = cam->setFormat(width, height, format); 40 | if(retval!=CAMSUCCESS) return -1; 41 | 42 | retval = cam->startStreaming(); 43 | 44 | if(retval==CAMSUCCESS) 45 | { 46 | cam->setColorMode(mode); 47 | unsigned int bytes; 48 | cam->grab(img, &bytes); 49 | 50 | if(format == V4L2_PIX_FMT_MJPEG) 51 | { 52 | char* path = (char*)"/tmp/test.jpg"; 53 | FILE* file = fopen (path, "wb"); 54 | fwrite (img, bytes, 1, file); 55 | fclose (file); 56 | } 57 | else 58 | { 59 | if(mode==CAMRGB) 60 | { 61 | char* path = (char*)"/tmp/test.ppm"; 62 | cout << "Serializing image to " << path << " (width: " << width << ", " << height << ")" << endl; 63 | serializePPM(path, img, width, height, 0); 64 | } 65 | else 66 | { 67 | char* path = (char*)"/tmp/test.pgm"; 68 | cout << "Serializing image to " << path << " (width: " << width << ", " << height << ")" << endl; 69 | serializePGM(path, img, width, height, 0); 70 | } 71 | } 72 | } 73 | else 74 | { 75 | cout << "Failed to connect to camera " << dev << endl; 76 | } 77 | 78 | delete [] img; 79 | delete cam; 80 | return 0; 81 | } 82 | -------------------------------------------------------------------------------- /obdevice/KinectPlayback.h: -------------------------------------------------------------------------------- 1 | #ifndef KINECTPLAYBACK_H 2 | #define KINECTPLAYBACK_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | /** 10 | * @namespace obvious 11 | */ 12 | namespace obvious 13 | { 14 | 15 | /** 16 | * @class KinectPlayback 17 | * @brief Kinect file interface 18 | * @author Stefan May 19 | **/ 20 | class KinectPlayback 21 | { 22 | public: 23 | /** 24 | * Standard constructor; 25 | */ 26 | KinectPlayback(const char* filename); 27 | 28 | /** 29 | * Standard destructor 30 | */ 31 | ~KinectPlayback(); 32 | 33 | /** 34 | * Grab new image 35 | * @return success 36 | */ 37 | bool grab(); 38 | 39 | /** 40 | * Signals end of file. Replay will start again from the beginning when calling reset. 41 | * @return end of file flag 42 | */ 43 | bool eof(); 44 | 45 | /** 46 | * Resets playback to the beginning of file stream. 47 | */ 48 | void reset(); 49 | 50 | /** 51 | * Skip a number of frames 52 | * @params frames number of frames 53 | */ 54 | void skip(unsigned int frames); 55 | 56 | /** 57 | * Get number of rows of images 58 | * @return rows 59 | */ 60 | unsigned int getRows(); 61 | 62 | /** 63 | * Get number of columns of images 64 | * @return columns 65 | */ 66 | unsigned int getCols(); 67 | 68 | /** 69 | * Accessor to point of coordinate data 70 | * @return pointer to coordinate buffer (layout x1y1z1x2...) 71 | */ 72 | double* getCoords(); 73 | 74 | /** 75 | * Accessor to mask of valid points 76 | * @return pointer to mask 77 | */ 78 | unsigned char* getMask(); 79 | 80 | /** 81 | * Accessor to array of distance data 82 | * @return pointer to z-array 83 | */ 84 | double* getZ(); 85 | 86 | /** 87 | * Accessor to point of color data 88 | * @return pointer to color buffer (layout r1g1b1r2...) 89 | */ 90 | unsigned char* getRGB(); 91 | 92 | private: 93 | double* _coords; 94 | double* _z; 95 | unsigned char* _rgb; 96 | unsigned char* _mask; 97 | 98 | unsigned int _rows; 99 | unsigned int _cols; 100 | 101 | bool _init; 102 | 103 | ifstream _recfile; 104 | unsigned int _frames; 105 | unsigned int _frame; 106 | 107 | bool _eof; 108 | }; 109 | 110 | } // end namespace 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /obvision/registration/icp/IcpMultiInitIterator.cpp: -------------------------------------------------------------------------------- 1 | #include "obvision/registration/icp/IcpMultiInitIterator.h" 2 | #include "obcore/base/Logger.h" 3 | 4 | namespace obvious 5 | { 6 | 7 | IcpMultiInitIterator::IcpMultiInitIterator(vector Tinit) 8 | { 9 | if(Tinit.size()==0) 10 | { 11 | LOGMSG(DBG_ERROR, "Initialization vector of length zero passed"); 12 | abort(); 13 | } 14 | 15 | for(vector::iterator it=Tinit.begin(); it!=Tinit.end(); it++) 16 | _Tinit.push_back(*it); 17 | 18 | _Tlast = NULL; 19 | } 20 | 21 | IcpMultiInitIterator::~IcpMultiInitIterator() 22 | { 23 | delete _Tlast; 24 | } 25 | 26 | inline bool assignBetterSolution(double &rmsBest, unsigned int &pairsBest, unsigned int &iterationsBest, Matrix &TBest, double rms, unsigned int pairs, unsigned int iterations, Matrix T) 27 | { 28 | if(pairs > pairsBest) 29 | { 30 | rmsBest = rms; 31 | pairsBest = pairs; 32 | iterationsBest = iterations; 33 | TBest = T; 34 | return true; 35 | } 36 | 37 | return false; 38 | } 39 | 40 | Matrix IcpMultiInitIterator::iterate(Icp* icp) 41 | { 42 | double rmsBest = 10e12; 43 | unsigned int pairsBest = 0; 44 | unsigned int iterationsBest = 10e4; 45 | Matrix TBest = icp->getFinalTransformation(); 46 | 47 | // Temporary results 48 | double rms; 49 | unsigned int pairs; 50 | unsigned int iterations; 51 | 52 | //unsigned int cnt = 0; 53 | 54 | for(vector::iterator it=_Tinit.begin(); it!=_Tinit.end(); it++) 55 | { 56 | icp->reset(); 57 | icp->iterate(&rms, &pairs, &iterations, &(*it)); 58 | assignBetterSolution(rmsBest, pairsBest, iterationsBest, TBest, rms, pairs, iterations, icp->getFinalTransformation()); 59 | 60 | /*char folder[32]; 61 | sprintf(folder, "trace_%d_%d", cnt++, retval); 62 | icp->serializeTrace(folder, 50);*/ 63 | } 64 | 65 | if(_Tlast) 66 | { 67 | // Apply matching with last transformation 68 | icp->reset(); 69 | icp->iterate(&rms, &pairs, &iterations); 70 | assignBetterSolution(rmsBest, pairsBest, iterationsBest, TBest, rms, pairs, iterations, *_Tlast); 71 | 72 | /*char folder[32]; 73 | sprintf(folder, "trace_%d_%d", cnt++, retval); 74 | icp->serializeTrace(folder, 50);*/ 75 | } 76 | else 77 | { 78 | _Tlast = new Matrix(TBest.getRows(), TBest.getCols()); 79 | } 80 | 81 | (*_Tlast) = TBest; 82 | 83 | return TBest; 84 | } 85 | 86 | } 87 | -------------------------------------------------------------------------------- /obcore/statemachine/Agent.h: -------------------------------------------------------------------------------- 1 | #ifndef AGENT_H_ 2 | #define AGENT_H_ 3 | 4 | #include "obcore/statemachine/states/StateBase.h" 5 | 6 | #include 7 | /** 8 | * @namespace obvious 9 | */ 10 | namespace obvious 11 | { 12 | 13 | /** 14 | * @class Agent 15 | * @brief Generic class for statemachine agents 16 | * @author Stefan May 17 | * @date 23.10.2014 18 | */ 19 | class Agent 20 | { 21 | 22 | public: 23 | 24 | /** 25 | * Constructor 26 | */ 27 | Agent(); 28 | 29 | /** 30 | * Destructor 31 | */ 32 | virtual ~Agent(); 33 | 34 | /** 35 | * Get Agent instance by passing unique ID. The Agent must be instantiated before, otherwise NULL is returned. 36 | * @param id unique agent ID (assigned when Agent is instantiated). 37 | * @return agent instance 38 | */ 39 | static Agent* getAgentByID(const unsigned int id); 40 | 41 | /** 42 | * Get unique ID of agent, each instance is assigned a sequential ID 43 | * @return ID 44 | */ 45 | unsigned int getID(); 46 | 47 | /** 48 | * Awake agent to do his job 49 | */ 50 | void awake(); 51 | 52 | /** 53 | * Register persistent state with given ID 54 | * @param stateID state ID 55 | * @param state state instance 56 | */ 57 | void registerPersistantState(const int stateID, StateBase* state); 58 | 59 | /** 60 | * Get state by ID 61 | * @param stateID 62 | * @return state instance 63 | */ 64 | StateBase* getPersistantState(const int stateID); 65 | 66 | /** 67 | * Activate previously registered state by ID 68 | * @param stateID state ID 69 | */ 70 | void transitionToPersistantState(const int stateID); 71 | 72 | /** 73 | * Activate new state instance. Instance is deleted with next transition. 74 | * @param nextState 75 | */ 76 | void transitionToVolatileState(StateBase* nextState); 77 | 78 | /** 79 | * Clean up persistent state map 80 | */ 81 | void deletePersistantStates(); 82 | 83 | private: 84 | 85 | unsigned int _ID; 86 | 87 | StateBase* _currentState; 88 | 89 | StateBase* _nextState; 90 | 91 | static unsigned int _AgentID; 92 | 93 | bool _volatile; 94 | 95 | bool _volatileNext; 96 | 97 | std::map _persistantStateMap; 98 | 99 | static std::map _agents; 100 | }; 101 | 102 | } // end namespace 103 | 104 | #endif 105 | -------------------------------------------------------------------------------- /obdevice/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | include(FindPkgConfig) 3 | 4 | ############################ 5 | # General project settings # 6 | ############################ 7 | 8 | PROJECT(OBDEVICE) 9 | SET(OBDEVICE_VERSION_MAJOR 0) 10 | SET(OBDEVICE_VERSION_MINOR 1) 11 | 12 | FIND_PACKAGE(Eigen REQUIRED) 13 | 14 | # source list 15 | SET(SOURCES 16 | Kinect.cpp 17 | KinectPlayback.cpp 18 | UvcCam.cpp 19 | UvcVirtualCam.cpp 20 | ParentDevice3D.cpp 21 | CloudFactory.cpp 22 | ) 23 | 24 | # if does exists find libopenni2 directories and libs 25 | EXECUTE_PROCESS(COMMAND pkg-config libopenni2 --exists RESULT_VARIABLE RET_CODE) 26 | IF(${RET_CODE} STREQUAL 0) 27 | MESSAGE(STATUS "Build with OpenNI2 ...") 28 | PKG_CHECK_MODULES(OPENNI libopenni2) 29 | INCLUDE_DIRECTORIES(${OPENNI_INCLUDE_DIRS}) 30 | SET(SOURCES ${SOURCES} OpenNiDevice.cpp) 31 | ENDIF() 32 | 33 | #FIND_PACKAGE(PCL 1.6 REQUIRED) 34 | #IF(PCL_FOUND) 35 | # SET(SOURCES ${SOURCES} PclCloudInterface.cpp) 36 | # MESSAGE(STATUS "Build with PCL interface ...") 37 | # INCLUDE_DIRECTORIES(${PCL_INCLUDE_DIRS}) 38 | # ADD_DEFINITIONS(${PCL_DEFINITIONS}) 39 | # LINK_DIRECTORIES(${PCL_LIBRARY_DIRS}) 40 | #ENDIF() 41 | 42 | INCLUDE_DIRECTORIES(/usr/include/openni 43 | /usr/include/ni 44 | $ENV{OBVIOUSLY_ROOT} 45 | ../obcore/math 46 | ../obcore 47 | ${EIGEN_INCLUDE_DIRS} 48 | ) 49 | 50 | # check if necessary files for PMD are installed 51 | IF(EXISTS /usr/local/include/PMDSDK) 52 | MESSAGE(STATUS "Build PMD stuff ...") 53 | INCLUDE_DIRECTORIES(/usr/local/include/PMDSDK) 54 | SET(SOURCES ${SOURCES} CamNano.cpp) 55 | ENDIF() 56 | 57 | # check if necessary files for sick lidar installed 58 | IF(EXISTS /usr/local/include/LMS1xx.h) 59 | message(STATUS "Build Sick stuff ...") 60 | set(SOURCES ${SOURCES} SickLMS100.cpp) 61 | ENDIF() 62 | 63 | # Disabled unknown pragmas and reordering due to OpenNI wrapper. Remove option, if wrapper is not used anymore 64 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D__OBVIOUSLY__ -Wno-unknown-pragmas -Wno-reorder") 65 | 66 | ADD_LIBRARY(obdevice STATIC ${SOURCES}) 67 | 68 | #################### 69 | ##### Packaging #### 70 | #################### 71 | IF(CMAKE_BUILD_TYPE MATCHES Release) 72 | INSTALL(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" DESTINATION ${OBVIOUSLY_INC_DIR} FILES_MATCHING PATTERN "*.h") 73 | INSTALL(TARGETS obdevice ARCHIVE DESTINATION ${OBVIOUSLY_LIB_DIR}) 74 | ENDIF() -------------------------------------------------------------------------------- /obvision/registration/icp/PointToLineEstimator2D.h: -------------------------------------------------------------------------------- 1 | #ifndef PointToLine2DEstimator_H 2 | #define PointToLine2DEstimator_H 3 | 4 | #include "obvision/registration/icp/IRigidEstimator.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class PointToLine2D 11 | * @brief An closed-form estimator to enregister 2D points clouds into one common coordinate system. 12 | * @author Christian Pfitzner 13 | */ 14 | class PointToLine2DEstimator : public IRigidEstimator 15 | { 16 | public: 17 | /** 18 | * Default constructor 19 | */ 20 | PointToLine2DEstimator(); 21 | /** 22 | * Destructor 23 | */ 24 | ~PointToLine2DEstimator (); 25 | /** 26 | * Setting internal pointer to model array. The model is seen as the ground truth against which the scene has to be registered. 27 | * @param model Pointer to 3 dimensional model array 28 | */ 29 | virtual void setModel(double** model, unsigned int size, double** normals); 30 | /** 31 | * Setting internal pointer to scene array. (See commend for setModel) 32 | * @param scene Pointer to 3 dimensional scene array 33 | */ 34 | virtual void setScene(double** scene, unsigned int size, double** normals=NULL); 35 | /** 36 | * Setting assigned point pairs. You can use a pair assigner for this purpose. The deviation, i.e. the mean distance, between those pairs is also determined within this method. 37 | * @param pairs Vector of pairs of indices. Each index pair references a scene and a model point. 38 | */ 39 | virtual void setPairs(std::vector* pairs); 40 | /** 41 | * Access the RMS error that has been calculated by the setPairs method. 42 | * @return RMS error 43 | */ 44 | virtual double getRMS(); 45 | /** 46 | * Determine the transformation matrix that registers the scene to the model. 47 | * @param T transformation matrix as return parameter 48 | */ 49 | virtual void estimateTransformation(Matrix* T); 50 | 51 | unsigned int getIterations(void); 52 | 53 | private: 54 | double** _model; //!< model 55 | double** _scene; //!< scene 56 | double** _normals; //!< pointer to normals 57 | std::vector* _pairs; //!< index pairs 58 | double _rms; //!< deviation 59 | unsigned int _iterations; 60 | double _cm[3]; 61 | double _cs[3]; 62 | }; 63 | 64 | } 65 | 66 | #endif /*PointToLine2DEstimator_H */ 67 | -------------------------------------------------------------------------------- /obvision/registration/icp/ClosedFormEstimator2D.h: -------------------------------------------------------------------------------- 1 | #ifndef CLOSEDFORMESTIMATOR2D_H_ 2 | #define CLOSEDFORMESTIMATOR2D_H_ 3 | 4 | #include "obvision/registration/icp/IRigidEstimator.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class ClosedFormEstimator2D 11 | * @brief An closed-form estimator to enregister 2D points clouds into one common coordinate system. 12 | * @author Stefan May 13 | */ 14 | class ClosedFormEstimator2D : public IRigidEstimator 15 | { 16 | public: 17 | /** 18 | * Default constructor 19 | */ 20 | ClosedFormEstimator2D(); 21 | 22 | /** 23 | * Destructor 24 | */ 25 | ~ClosedFormEstimator2D(); 26 | 27 | /** 28 | * Setting internal pointer to model array. The model is seen as the ground truth against which the scene has to be registered. 29 | * @param model Pointer to 3 dimensional model array 30 | */ 31 | virtual void setModel(double** model, unsigned int size, double** normals=NULL); 32 | 33 | /** 34 | * Setting internal pointer to scene array. (See commend for setModel) 35 | * @param scene Pointer to 3 dimensional scene array 36 | */ 37 | virtual void setScene(double** scene, unsigned int size, double** normals=NULL); 38 | 39 | /** 40 | * Setting assigned point pairs. You can use a pair assigner for this purpose. The deviation, i.e. the mean distance, between those pairs is also determined within this method. 41 | * @param pairs Vector of pairs of indices. Each index pair references a scene and a model point. 42 | */ 43 | virtual void setPairs(std::vector* pairs); 44 | 45 | /** 46 | * Access the RMS error that has been calculated by the setPairs method. 47 | * @return RMS error 48 | */ 49 | virtual double getRMS(); 50 | 51 | /** 52 | * Determine the transformation matrix that registers the scene to the model. 53 | * @param T transformation matrix as return parameter 54 | */ 55 | virtual void estimateTransformation(Matrix* T); 56 | 57 | private: 58 | 59 | /** 60 | * Centroid of model 61 | */ 62 | double _cm[2]; 63 | 64 | /** 65 | * Model 66 | */ 67 | double** _model; 68 | 69 | /** 70 | * Centroid of scene 71 | */ 72 | double _cs[2]; 73 | 74 | /** 75 | * Scene 76 | */ 77 | double** _scene; 78 | 79 | /** 80 | * Index pairs 81 | */ 82 | std::vector* _pairs; 83 | 84 | /** 85 | * Deviation 86 | */ 87 | double _rms; 88 | 89 | unsigned int _iterations; 90 | 91 | }; 92 | 93 | } 94 | 95 | #endif /*CLOSEDFORMESTIMATOR2D_H_*/ 96 | -------------------------------------------------------------------------------- /obcore/filter/NormalFilter.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file NormalFilter.cpp 3 | * @autor christian 4 | * @date 04.12.2012 5 | * 6 | * 7 | */ 8 | 9 | #include "obcore/filter/NormalFilter.h" 10 | 11 | using namespace obvious; 12 | 13 | /* 14 | * Function to start filtering 15 | */ 16 | FILRETVAL NormalFilter::applyFilter(void) 17 | { 18 | if((!_input)||(!_output)||(!_inputNormals)||(!_outputNormals)) 19 | { 20 | LOGMSG(DBG_ERROR, "Pointer to input, output or normals invalid"); 21 | return(FILTER_ERROR); 22 | } 23 | // check if custom axis is set 24 | if (_customAxis == CustomAxis(0,0,0)) 25 | { 26 | if (_axis == x) 27 | _customAxis = CustomAxis(1,0,0); 28 | else if (_axis == y) 29 | _customAxis = CustomAxis(0,1,0); 30 | else if (_axis == z) 31 | _customAxis = CustomAxis(0,0,1); 32 | } 33 | 34 | _validSize = 0; 35 | double *dPtr = _input; 36 | double *dPtrN = _inputNormals; 37 | 38 | // init output arrays 39 | for(unsigned int i=0 ; i<_size ; i++) 40 | { 41 | _output[i] = 0.0; 42 | _outputNormals[i] = 0.0; 43 | } 44 | 45 | if (_direction == FILTER_BIGGER) 46 | { 47 | for(unsigned int i=0 ; i<_size ; i+=3) { 48 | double normal[3] = {_inputNormals[i], _inputNormals[i+1], _inputNormals[i+2]}; 49 | double axis[3] = {_customAxis[0], _customAxis[1], _customAxis[2]}; 50 | double angle = getAngleBetweenVec(normal, axis); 51 | 52 | if(angle > _threshold) 53 | { 54 | dPtr += 3; 55 | dPtrN += 3; 56 | } 57 | else 58 | { 59 | for(unsigned int j=0 ; j<3 ; j++) 60 | { 61 | *_output++ = *dPtr++; 62 | *_outputNormals++ = *dPtr++; 63 | } 64 | _validSize += 3; 65 | } 66 | } 67 | } 68 | else // FILTER_SMALLER 69 | { 70 | for(unsigned int i=0 ; i<_size ; i+=3) { 71 | double normal[3] = {_inputNormals[i], _inputNormals[i+1], _inputNormals[i+2]}; 72 | double axis[3] = {_customAxis[0], _customAxis[1], _customAxis[2]}; 73 | double angle = getAngleBetweenVec(normal, axis); 74 | 75 | if(angle <= _threshold) 76 | { 77 | dPtr += 3; 78 | dPtrN += 3; 79 | } 80 | else 81 | { 82 | for(unsigned int j=0 ; j<3 ; j++) 83 | { 84 | *_output++ = *dPtr++; 85 | *_outputNormals++ = *dPtr++; 86 | } 87 | _validSize += 3; 88 | } 89 | } 90 | } 91 | return(FILTER_OK); 92 | } 93 | 94 | 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /obgraphic/Obvious2DMap.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Obvious2DMap.cpp 3 | * @author Christian Pfitzner 4 | * @date 12.01.2013 5 | * 6 | * 7 | */ 8 | 9 | // includes of obviously 10 | #include "obgraphic/Obvious2DMap.h" 11 | #include "obcore/math/mathbase.h" 12 | // includes of glut 13 | #define GL_GLEXT_PROTOTYPES 1 14 | #include 15 | 16 | 17 | using namespace obvious; 18 | 19 | void Obvious2DMap::draw(unsigned char* image, unsigned int width, unsigned int height, unsigned int channels) 20 | { 21 | if(_gridActive) 22 | drawGrid(); 23 | 24 | if(_circleActive) 25 | drawCircle(); 26 | 27 | if(_anglesActive) 28 | drawAngles(); 29 | 30 | drawCenter(); 31 | drawDefault(image, width, height, channels); 32 | } 33 | 34 | 35 | void Obvious2DMap::drawGrid(void) const 36 | { 37 | glColor3f(.15,.15,.15); 38 | glBegin(GL_LINES); 39 | for(int i=-10 ; i<10 ; i+=2) 40 | { 41 | // horizontal lines 42 | glVertex3f(-1,i*0.1,0); 43 | glVertex3f(1,i*0.1,0); 44 | // vertical lines 45 | glVertex3f(i*0.1,-1,0); 46 | glVertex3f(i*0.1,1,0); 47 | } 48 | glEnd(); 49 | } 50 | 51 | void Obvious2DMap::drawCircle(void) const 52 | { 53 | const int sides = 36; // The amount of segment to create the circle 54 | float step = 1/(_lengthMap); 55 | unsigned int i=1; 56 | for(float r=step ; r<_lengthMap ; r+=step) 57 | { 58 | if (i%2 == 0) 59 | glColor3f(.3,.3,.3); 60 | else 61 | glColor3f(.08,.08,.08); 62 | 63 | glBegin(GL_LINE_LOOP); 64 | for (int a = 0; a < 360; a += 360 / sides) 65 | { 66 | double heading = a * M_PI / 180; 67 | glVertex2d(cos(heading)*r, sin(heading)*r); 68 | } 69 | glEnd(); 70 | i++; 71 | } 72 | } 73 | 74 | 75 | void Obvious2DMap::drawAngles(const float angleStep) const 76 | { 77 | glBegin(GL_LINES); 78 | for(int i=0 ; i<360 ; i+=angleStep) 79 | { 80 | if (i%90 == 0) 81 | glColor3f(.3,.3,.3); 82 | else 83 | glColor3f(.08,.08,.08); 84 | double heading = i * M_PI / 180; 85 | glVertex2d(cos(heading)*2, sin(heading)*2); 86 | glVertex2d(0,0); 87 | } 88 | glEnd(); 89 | } 90 | 91 | void Obvious2DMap::drawCenter(void) const 92 | { 93 | glBegin(GL_TRIANGLES); 94 | glColor3f(.6,0,0); 95 | glVertex2f(0,0.03); 96 | glVertex2f(-0.01, -0.03); 97 | glVertex2f(0.01, -0.03); 98 | glEnd(); 99 | glColor3f(.15,.15,.15); 100 | 101 | glutSwapBuffers(); 102 | glutMainLoopEvent(); 103 | } 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/OcclusionFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "OcclusionFilter.h" 2 | 3 | #include "obcore/base/System.h" 4 | #include "obcore/base/tools.h" 5 | #include 6 | #include 7 | #include 8 | 9 | namespace obvious 10 | { 11 | 12 | OcclusionFilter::OcclusionFilter(double* P, unsigned int width, unsigned int height) 13 | { 14 | System::allocate(height, width, _zbuffer); 15 | System::allocate(height, width, _map); 16 | 17 | memcpy(_P, P, 12*sizeof(*_P)); 18 | 19 | _w = width; 20 | _h = height; 21 | }; 22 | 23 | OcclusionFilter::~OcclusionFilter() 24 | { 25 | System::deallocate(_zbuffer); 26 | System::deallocate(_map); 27 | }; 28 | 29 | void OcclusionFilter::update(double* P) 30 | { 31 | memcpy(_P, P, 12*sizeof(*_P)); 32 | } 33 | 34 | void OcclusionFilter::filter(double** scene, unsigned int size, bool* mask) 35 | { 36 | if(!_active) return; 37 | 38 | #ifndef NDEBUG 39 | unsigned char* proj = new unsigned char[3*_w * _h]; 40 | memset(proj, 0, 3*_w*_h*sizeof(*proj)); 41 | #endif 42 | 43 | for(unsigned int i=0; i<_w*_h; i++) 44 | { 45 | _map[0][i] = -1; 46 | _zbuffer[0][i] = 10e6; 47 | } 48 | for(unsigned int i=0; i1e-12 && scene[i][2] > 0) 53 | { 54 | double du = (_P[0] * scene[i][0] + _P[1] * scene[i][1] + _P[2] * scene[i][2] + _P[3]) / dw; 55 | double dv = (_P[4] * scene[i][0] + _P[5] * scene[i][1] + _P[6] * scene[i][2] + _P[7]) / dw; 56 | int u = (int)(du + 0.5); 57 | int v = _h-1-(int)(dv + 0.5); 58 | if((u>=0) && (u<(int)_w) && (v>=0) && (v<(int)_h)) 59 | { 60 | int mval = _map[v][u]; 61 | // Two points intersect 62 | if(mval>-1) 63 | { 64 | if((_zbuffer[v][u] - scene[i][2]) > 1e-3) 65 | { 66 | mask[mval] = 0; 67 | _map[v][u] = i; 68 | _zbuffer[v][u] = scene[i][2]; 69 | } 70 | else if((_zbuffer[v][u] - scene[i][2]) < -1e-3) 71 | { 72 | mask[i] = 0; 73 | } 74 | } 75 | else 76 | { 77 | _map[v][u] = i; 78 | _zbuffer[v][u] = scene[i][2]; 79 | } 80 | #ifndef NDEBUG 81 | int idx = v*_w + u; 82 | proj[3*idx]=255; 83 | proj[3*idx+1]=255; 84 | proj[3*idx+2]=255; 85 | #endif 86 | } 87 | } 88 | } 89 | #ifndef NDEBUG 90 | serializePPM("/tmp/projo.ppm", proj, _w, _h); 91 | delete [] proj; 92 | #endif 93 | } 94 | 95 | } 96 | -------------------------------------------------------------------------------- /obcore/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | include(FindPkgConfig) 3 | 4 | ############################ 5 | # General project settings # 6 | ############################ 7 | 8 | PROJECT(OBCORE) 9 | 10 | SET(USE_EIGEN 0) 11 | CONFIGURE_FILE( math/linalg/linalg.h.in ${CMAKE_CURRENT_SOURCE_DIR}/math/linalg/linalg.h ) 12 | 13 | FIND_PACKAGE(Eigen REQUIRED) 14 | 15 | INCLUDE_DIRECTORIES(../.. 16 | .. 17 | . 18 | ${EIGEN_INCLUDE_DIRS} 19 | /usr/include/lua5.1 20 | ) 21 | 22 | SET(LIBRARY STATIC 23 | base/CartesianCloud.cpp 24 | base/CartesianCloudFactory.cpp 25 | base/Timer.cpp 26 | base/Logger.cpp 27 | base/tools.cpp 28 | base/Time.cpp 29 | base/PointCloud.cpp 30 | statemachine/Agent.cpp 31 | statemachine/AgentModel.cpp 32 | statemachine/RobotModel.cpp 33 | statemachine/states/StateBase.cpp 34 | statemachine/states/StateLua.cpp 35 | statemachine/states/StatePing.cpp 36 | statemachine/states/StatePong.cpp 37 | math/geometry.cpp 38 | math/linalg/MatrixFactory.cpp 39 | math/Quaternion.cpp 40 | math/PID_Controller.cpp 41 | math/IntegratorSimpson.cpp 42 | math/TransformationWatchdog.cpp 43 | math/Trajectory.cpp 44 | filter/EuclideanFilter.cpp 45 | filter/CartesianFilter.cpp 46 | filter/NormalFilter.cpp 47 | filter/BoundingBoxFilter.cpp 48 | scripting/LuaScriptManager.cpp 49 | ) 50 | 51 | IF(USE_EIGEN) 52 | message("Using libeigen for obcore") 53 | SET(LIBRARY ${LIBRARY} math/linalg/eigen/Matrix.cpp 54 | math/linalg/eigen/Vector.cpp 55 | ) 56 | ELSE() 57 | message("Using gsl for obcore") 58 | 59 | SET(LIBRARY ${LIBRARY} math/linalg/gsl/Matrix.cpp 60 | math/linalg/gsl/Vector.cpp 61 | ) 62 | ENDIF() 63 | 64 | ADD_LIBRARY(obcore ${LIBRARY}) 65 | 66 | #################### 67 | ##### Packaging #### 68 | #################### 69 | IF(CMAKE_BUILD_TYPE MATCHES Release) 70 | INSTALL(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" DESTINATION ${OBVIOUSLY_INC_DIR} FILES_MATCHING PATTERN "*.h") 71 | INSTALL(TARGETS obcore ARCHIVE DESTINATION ${OBVIOUSLY_LIB_DIR}) 72 | ENDIF() -------------------------------------------------------------------------------- /obcore/base/CartesianCloudFactory.cpp: -------------------------------------------------------------------------------- 1 | #include "CartesianCloudFactory.h" 2 | #include 3 | #include 4 | 5 | #include "obcore/base/Logger.h" 6 | 7 | namespace obvious 8 | { 9 | 10 | CartesianCloud3D* loadAscii(char* filename); 11 | void serializeAscii(char* filename, CartesianCloud3D* cloud); 12 | 13 | CartesianCloud3D* CartesianCloudFactory::load(char* filename, EnumFileFormat format) 14 | { 15 | switch (format) 16 | { 17 | case eFormatAscii: 18 | return loadAscii(filename); 19 | break; 20 | default: 21 | LOGMSG(DBG_ERROR, "File format unknown"); 22 | return NULL; 23 | break; 24 | } 25 | } 26 | 27 | void CartesianCloudFactory::serialize(char* filename, CartesianCloud3D* cloud, EnumFileFormat format) 28 | { 29 | switch (format) 30 | { 31 | case eFormatAscii: 32 | return serializeAscii(filename, cloud); 33 | break; 34 | } 35 | } 36 | 37 | void serializeAscii(char* filename, CartesianCloud3D* cloud) 38 | { 39 | ofstream file; 40 | file.open(filename, ios::out); 41 | 42 | // Number of points 43 | unsigned int cnt = cloud->size(); 44 | unsigned char* colors = cloud->getColors(); 45 | 46 | for(unsigned int i=0; igetCoords()))(i,0) << " " << (*(cloud->getCoords()))(i,1) << " " << (*(cloud->getCoords()))(i,2) << " "; 49 | if(colors) file << (unsigned int)colors[0] << " " << (unsigned int)colors[1] << " " << (unsigned int)colors[2] << endl; 50 | } 51 | 52 | file.close(); 53 | } 54 | 55 | CartesianCloud3D* loadAscii(char* filename) 56 | { 57 | ifstream file; 58 | file.open(filename, ios::in); 59 | 60 | // Determine number of points 61 | unsigned int size = 0; 62 | string line; 63 | while (getline(file, line)) 64 | size++; 65 | file.clear(); 66 | file.seekg(0); 67 | 68 | CartesianCloud3D* cloud = new CartesianCloud3D(size, true); 69 | double x, y, z, r, g, b; 70 | int cnt = 0; 71 | unsigned char* colors = cloud->getColors(); 72 | int* attributes = cloud->getAttributes(); 73 | int* sourceidx = cloud->getIndices(); 74 | 75 | file >> x >> y >> z >> r >> g >> b; 76 | while (file.good()) 77 | { 78 | (*(cloud->getCoords()))(cnt,0) = x; 79 | (*(cloud->getCoords()))(cnt,1) = y; 80 | (*(cloud->getCoords()))(cnt,2) = z; 81 | colors[3 * cnt] = r; 82 | colors[3 * cnt + 1] = g; 83 | colors[3 * cnt + 2] = b; 84 | attributes[cnt] = 0; 85 | if (z > 0) attributes[cnt] |= ePointAttrValid; 86 | sourceidx[cnt] = cnt; 87 | cnt++; 88 | file >> x >> y >> z >> r >> g >> b; 89 | } 90 | 91 | return cloud; 92 | } 93 | 94 | } 95 | -------------------------------------------------------------------------------- /obvision/planning/AStarNode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * AStarNode.h 3 | * 4 | * Created on: 03.11.2014 5 | * Author: Jon Martin and Stefan May 6 | */ 7 | 8 | #ifndef ASTARNODE_H_ 9 | #define ASTARNODE_H_ 10 | 11 | namespace obvious 12 | { 13 | 14 | class AStarNode 15 | { 16 | 17 | friend class AStar; 18 | 19 | public: 20 | 21 | /** 22 | * Constructor 23 | * @param xp 24 | * @param yp 25 | * @param d 26 | * @param p 27 | */ 28 | AStarNode(int xp, int yp, int d, int p, int currentDir); 29 | 30 | /** 31 | * Destructor 32 | */ 33 | virtual ~AStarNode(); 34 | 35 | /** 36 | * Set current direction 37 | * @param dir direction 38 | */ 39 | void setCurrentDir(int dir); 40 | 41 | /** 42 | * Get current direction 43 | * @return current direction 44 | */ 45 | int getCurrentDir() const; 46 | 47 | /** 48 | * 49 | * @return 50 | */ 51 | int getxPos() const; 52 | 53 | /** 54 | * 55 | * @return 56 | */ 57 | int getyPos() const; 58 | 59 | /** 60 | * 61 | * @return 62 | */ 63 | int getLevel() const; 64 | 65 | /** 66 | * 67 | * @return 68 | */ 69 | int getPriority() const; 70 | 71 | /** 72 | * 73 | * @param xDest 74 | * @param yDest 75 | */ 76 | void updatePriority(const int & xDest, const int & yDest); 77 | 78 | /** 79 | * give better priority to going strait instead of diagonally 80 | * @param i direction 81 | */ 82 | void nextLevel(const int & i); 83 | 84 | /** 85 | * Calculate next level with directional penalty 86 | * @param i direction 87 | */ 88 | void nextLevelPenalty(const int & i); 89 | 90 | /** 91 | * Estimation function for the remaining distance to the goal. 92 | * @param xDest 93 | * @param yDest 94 | */ 95 | const int estimate(const int & xDest, const int & yDest) const; 96 | 97 | private: 98 | 99 | /** 100 | * Priority queues overwritten flag 101 | */ 102 | bool _isOverwritten; 103 | 104 | /** 105 | * current position 106 | */ 107 | int _xPos; 108 | 109 | /** 110 | * 111 | */ 112 | int _yPos; 113 | 114 | /** 115 | * total distance already travelled to reach the node 116 | */ 117 | int _level; 118 | 119 | /** 120 | * priority=level+remaining distance estimate+heuristic 121 | * smaller: higher priority 122 | */ 123 | int _priority; 124 | 125 | /** 126 | * current direction 127 | */ 128 | int _currentDir; 129 | }; 130 | 131 | } /* namespace obvious */ 132 | #endif /* ASTARNODE_H_ */ 133 | -------------------------------------------------------------------------------- /obvision/registration/icp/assign/filter/ReciprocalFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "ReciprocalFilter.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace obvious 7 | { 8 | 9 | struct StrReciprocalPair 10 | { 11 | unsigned int unIdx; 12 | unsigned int unModelIndex; 13 | double dDistanceSqr; 14 | }; 15 | 16 | bool operator < (const StrReciprocalPair& first, const StrReciprocalPair& second) 17 | { 18 | if (first.unModelIndex < second.unModelIndex) return true; 19 | else if ( (first.unModelIndex == second.unModelIndex) && (first.dDistanceSqr < second.dDistanceSqr) ) return true; 20 | return false; 21 | } 22 | 23 | ReciprocalFilter::ReciprocalFilter() 24 | { 25 | }; 26 | 27 | ReciprocalFilter::~ReciprocalFilter() 28 | { 29 | 30 | }; 31 | 32 | void ReciprocalFilter::filter(double** model, double** scene, vector* pairs, vector* distancesSqr, vector* fpairs, vector* fdistancesSqr, vector* nonPairs) 33 | { 34 | if(!_active) 35 | { 36 | *fpairs = *pairs; 37 | *fdistancesSqr = *distancesSqr; 38 | return; 39 | } 40 | 41 | fpairs->clear(); 42 | fdistancesSqr->clear(); 43 | 44 | // sort pairs w.r.t model point index (and distance if model point indices are equal) 45 | std::vector vReciprocalPairs; 46 | unsigned int i=0; 47 | for( i=0; i < pairs->size(); ++i) 48 | { 49 | StrReciprocalPair pair; 50 | pair.unIdx = i; 51 | pair.unModelIndex = (*pairs)[i].indexFirst; 52 | pair.dDistanceSqr = (*distancesSqr)[i]; 53 | vReciprocalPairs.push_back(pair); 54 | } 55 | 56 | if(vReciprocalPairs.size()==0) return; 57 | 58 | std::sort(vReciprocalPairs.begin(), vReciprocalPairs.end()); 59 | 60 | unsigned int unModelIndexLast = vReciprocalPairs[0].unModelIndex; // remember model point index of first pair 61 | fpairs->push_back((*pairs)[vReciprocalPairs[0].unIdx]); // store first pair in result vector 62 | for ( i=1; i < vReciprocalPairs.size(); ++i ) 63 | { 64 | // ignore all pairs containing the same model point index 65 | if ( vReciprocalPairs[i].unModelIndex == unModelIndexLast) 66 | { 67 | unsigned int idx = vReciprocalPairs[i].unIdx; 68 | nonPairs->push_back((*pairs)[idx].indexSecond); 69 | } 70 | else 71 | { 72 | unModelIndexLast = vReciprocalPairs[i].unModelIndex; 73 | unsigned int idx = vReciprocalPairs[i].unIdx; 74 | fpairs->push_back((*pairs)[idx]); 75 | fdistancesSqr->push_back((*distancesSqr)[idx]); 76 | } 77 | } 78 | } 79 | 80 | } 81 | -------------------------------------------------------------------------------- /obcore/math/linalg/MatrixFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef MATRIXFACTORY_H__ 2 | #define MATRIXFACTORY_H__ 3 | 4 | using namespace std; 5 | 6 | #include "obcore/base/types.h" 7 | 8 | /** 9 | * @namespace obvious 10 | */ 11 | namespace obvious 12 | { 13 | 14 | class Matrix; 15 | 16 | /** 17 | * @class MatrixFactory 18 | * @author Stefan May 19 | */ 20 | class MatrixFactory 21 | { 22 | public: 23 | 24 | /** 25 | * Instantiate a 2x2 roation matrix 26 | * @param phi rotation about z-axis 27 | * @return matrix instance 28 | */ 29 | static Matrix RotationMatrix22(obfloat phi); 30 | 31 | /** 32 | * Instantiate a 3x3 roation matrix 33 | * @param psi rotation about x-axis 34 | * @return matrix instance 35 | */ 36 | static Matrix RotationMatrix33AboutX(obfloat psi); 37 | 38 | /** 39 | * Instantiate a 3x3 roation matrix 40 | * @param theta rotation about y-axis 41 | * @return matrix instance 42 | */ 43 | static Matrix RotationMatrix33AboutY(obfloat theta); 44 | 45 | /** 46 | * Instantiate a 3x3 roation matrix 47 | * @param phi rotation about z-axis 48 | * @return matrix instance 49 | */ 50 | static Matrix RotationMatrix33AboutZ(obfloat phi); 51 | 52 | /** 53 | * Instantiate a 4x4 translation matrix, i.e. identity with last column set to translational input 54 | * @param tx x-component of translation 55 | * @param ty y-component of translation 56 | */ 57 | static Matrix TranslationMatrix33(obfloat tx, obfloat ty); 58 | 59 | /** 60 | * Instantiate a 4x4 translation matrix, i.e. identity with last column set to translational input 61 | * @param tx x-component of translation 62 | * @param ty y-component of translation 63 | * @param tz z-component of translation 64 | */ 65 | static Matrix TranslationMatrix44(obfloat tx, obfloat ty, obfloat tz); 66 | 67 | /** 68 | * Instantiate a 3x3 transformation matrix 69 | * @param phi rotation about z-axis 70 | * @param tx x-component of translation 71 | * @param ty y-component of translation 72 | */ 73 | static Matrix TransformationMatrix33(obfloat phi, obfloat tx, obfloat ty); 74 | 75 | 76 | /** 77 | * Instantiate a 4x4 transformation matrix 78 | * @param phi rotation about z-axis 79 | * @param theta rotation about y-axis 80 | * @param psi rotation about x-axis 81 | * @param tx x-component of translation 82 | * @param ty y-component of translation 83 | * @param tz z-component of translation 84 | */ 85 | static Matrix TransformationMatrix44(obfloat phi, obfloat theta, obfloat psi, obfloat tx=0, obfloat ty=0, obfloat tz=0); 86 | 87 | private: 88 | 89 | }; 90 | 91 | } 92 | 93 | #endif //MATRIXFACTORY_H 94 | -------------------------------------------------------------------------------- /obcore/statemachine/RobotModel.cpp: -------------------------------------------------------------------------------- 1 | #include "RobotModel.h" 2 | #include 3 | 4 | namespace obvious 5 | { 6 | 7 | RobotModel::RobotModel(Point pos) 8 | { 9 | _pos = pos; 10 | _orientation[0] = 0; 11 | _orientation[1] = 0; 12 | _orientation[2] = 0; 13 | } 14 | 15 | RobotModel::RobotModel(Point2D pos) 16 | { 17 | _pos.x = pos.x; 18 | _pos.y = pos.y; 19 | _pos.z = 0; 20 | _orientation[0] = 0; 21 | _orientation[1] = 0; 22 | _orientation[2] = 0; 23 | } 24 | 25 | RobotModel::~RobotModel() 26 | { 27 | 28 | } 29 | 30 | bool RobotModel::isPoseUpToDate(const double interval) 31 | { 32 | return (_timer.elapsed() <= interval); 33 | } 34 | 35 | void RobotModel::setPosition(Point pos) 36 | { 37 | _timer.reset(); 38 | _pos = pos; 39 | } 40 | 41 | void RobotModel::setPosition(Point2D pos) 42 | { 43 | _timer.reset(); 44 | _pos.x = pos.x; 45 | _pos.y = pos.y; 46 | _pos.z = 0; 47 | } 48 | 49 | void RobotModel::setPosition(double x, double y, double z) 50 | { 51 | _timer.reset(); 52 | _pos.x = x; 53 | _pos.y = y; 54 | _pos.z = z; 55 | } 56 | 57 | void RobotModel::getPosition(Point& pos) 58 | { 59 | pos = _pos; 60 | } 61 | 62 | void RobotModel::getPosition(Point2D& pos) 63 | { 64 | pos.x = _pos.x; 65 | pos.y = _pos.y; 66 | } 67 | 68 | void RobotModel::getPosition(double& x, double& y, double& z) 69 | { 70 | x = _pos.x; 71 | y = _pos.y; 72 | z = _pos.z; 73 | } 74 | 75 | void RobotModel::setOrientation2D(double phi) 76 | { 77 | _orientation[0] = 0.0; 78 | _orientation[1] = 0.0; 79 | _orientation[2] = phi; 80 | } 81 | 82 | void RobotModel::getOrientation(double orientation[3]) 83 | { 84 | memcpy(orientation, _orientation, 3*sizeof(*_orientation)); 85 | } 86 | 87 | void RobotModel::setPath(std::vector path) 88 | { 89 | _path = path; 90 | } 91 | 92 | void RobotModel::getPath(std::vector& path) 93 | { 94 | path = _path; 95 | } 96 | 97 | void RobotModel::clearPath() 98 | { 99 | _path.clear(); 100 | } 101 | 102 | void RobotModel::pushTarget(obvious::Point2D target) 103 | { 104 | _targets.push_back(target); 105 | } 106 | 107 | bool RobotModel::getNextTarget(obvious::Point2D& target) 108 | { 109 | if(_targets.size()==0) return false; 110 | target = *_targets.begin(); 111 | 112 | return true; 113 | } 114 | 115 | bool RobotModel::popNextTarget(obvious::Point2D& target) 116 | { 117 | if(_targets.size()==0) return false; 118 | 119 | target = _targets[0]; 120 | _targets.erase(_targets.begin()); 121 | 122 | return true; 123 | } 124 | 125 | } // end namespace 126 | 127 | -------------------------------------------------------------------------------- /obvision/reconstruct/space/RayCast3D.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCAST3D_H 2 | #define RAYCAST3D_H 3 | 4 | #include 5 | #include "obcore/math/linalg/linalg.h" 6 | #include "TsdSpace.h" 7 | 8 | namespace obvious 9 | { 10 | 11 | enum AXSPARMODE 12 | { 13 | X_AXS, 14 | Y_AXS, 15 | Z_AXS, 16 | }; 17 | 18 | /** 19 | * @class RayCast3D 20 | * @brief Implementation of axis-parallel ray casting methods 21 | * @author Philipp Koch, Stefan May 22 | */ 23 | class RayCast3D 24 | { 25 | public: 26 | 27 | /** 28 | * Constructor 29 | */ 30 | RayCast3D(); 31 | 32 | /** 33 | * Destructor 34 | */ 35 | virtual ~RayCast3D(); 36 | 37 | /** 38 | * 39 | * @param space 40 | * @param sensor 41 | * @param coords 42 | * @param normals 43 | * @param rgb 44 | * @param size 45 | */ 46 | virtual void calcCoordsFromCurrentPose(TsdSpace* space, Sensor* sensor, double* coords, double* normals, unsigned char* rgb, unsigned int* size); 47 | 48 | /** 49 | * 50 | * @param space 51 | * @param sensor 52 | * @param coords 53 | * @param normals 54 | * @param rgb 55 | * @param mask 56 | * @param size 57 | */ 58 | virtual void calcCoordsFromCurrentPoseMask(TsdSpace* space, Sensor* sensor, double* coords, double* normals, unsigned char* rgb, bool* mask, unsigned int* size); 59 | 60 | /** 61 | * Overloaded method to cast a single ray trough several spaces. The method returns in case of a found coordinate or at 62 | * the end of the TsdSpace input vector. 63 | * @param sensor Pointer to sensor 64 | * @param coords Coordinates of found point 65 | * @param normals Normals of found point 66 | * @param rgb Color of found point 67 | * @param spaces Input vector with TsdSpaces to traverse 68 | * @param offsets Offset vector of the spaces in world coordinates system 69 | * @param u Coloumn the ray is casted through 70 | * @param v Row the ray is casted through 71 | * @return True in case of a found point, false otherwise 72 | */ 73 | /*virtual bool calcCoordsFromCurrentPose(TsdSpace* space, Sensor* sensor, double* coords, double* normals, unsigned char* rgb, const std::vector& spaces, 74 | const std::vector& offsets, const unsigned int u, const unsigned int v);*/ 75 | 76 | 77 | private: 78 | 79 | bool rayCastFromSensorPose(TsdSpace* space, obfloat pos[3], obfloat ray[3], obfloat coordinates[3], obfloat normal[3], unsigned char rgb[3], obfloat* depth); 80 | 81 | obfloat _xmin; 82 | obfloat _ymin; 83 | obfloat _zmin; 84 | 85 | obfloat _xmax; 86 | obfloat _ymax; 87 | obfloat _zmax; 88 | 89 | obfloat _idxMin; 90 | obfloat _idxMax; 91 | }; 92 | 93 | } 94 | 95 | #endif //RAYCAST3D 96 | -------------------------------------------------------------------------------- /obvision/registration/icp/PointToPlaneEstimator3D.h: -------------------------------------------------------------------------------- 1 | #ifndef POINTTOPLANEESTIMATOR3D_H_ 2 | #define POINTTOPLANEESTIMATOR3D_H_ 3 | 4 | #include "obvision/registration/icp/IRigidEstimator.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class PointToPlaneEstimator3D 11 | * @brief An estimator for registering points clouds into one common coordinate system. This estimator is based on a point-to-plane metric. 12 | * @author Stefan May 13 | */ 14 | class PointToPlaneEstimator3D : public IRigidEstimator 15 | { 16 | public: 17 | /** 18 | * Default constructor 19 | */ 20 | PointToPlaneEstimator3D(); 21 | 22 | /** 23 | * Destructor 24 | */ 25 | ~PointToPlaneEstimator3D(); 26 | 27 | /** 28 | * Setting internal pointer to model array. The model is seen as the ground truth against which the scene has to be registered. 29 | * @param model Pointer to 3 dimensional model array 30 | */ 31 | virtual void setModel(double** model, unsigned int size, double** normals); 32 | 33 | /** 34 | * Setting internal pointer to scene array. (See commend for setModel) 35 | * @param scene Pointer to 3 dimensional scene array 36 | */ 37 | virtual void setScene(double** scene, unsigned int size, double** normals=NULL); 38 | 39 | /** 40 | * Setting assigned point pairs. You can use a pair assigner for this purpose. The deviation, i.e. the mean distance, between those pairs is also determined within this method. 41 | * @param pairs Vector of pairs of indices. Each index pair references a scene and a model point. 42 | */ 43 | virtual void setPairs(std::vector* pairs); 44 | 45 | /** 46 | * Access the root mean square error that has been calculated by the setPairs method. 47 | * @return RMS error 48 | */ 49 | virtual double getRMS(); 50 | 51 | virtual unsigned int getIterations(void); 52 | 53 | /** 54 | * Determine the transformation matrix that registers the scene to the model. 55 | * @param T transformation matrix as return parameter 56 | */ 57 | virtual void estimateTransformation(Matrix* T); 58 | 59 | 60 | 61 | private: 62 | 63 | /** 64 | * Pointer to model 65 | */ 66 | double** _model; 67 | 68 | /** 69 | * Pointer to normals 70 | */ 71 | double** _normals; 72 | 73 | /** 74 | * Pointer to scene 75 | */ 76 | double** _scene; 77 | 78 | /** 79 | * Root mean square error 80 | */ 81 | double _rms; 82 | unsigned int _iterations; 83 | 84 | /** 85 | * Index pairs 86 | */ 87 | std::vector* _pairs; 88 | }; 89 | 90 | } 91 | 92 | #endif /*POINTTOPLANEESTIMATOR3D_H_*/ 93 | -------------------------------------------------------------------------------- /obcore/base/System.h: -------------------------------------------------------------------------------- 1 | #ifndef SYSTEM_H_ 2 | #define SYSTEM_H_ 3 | 4 | #include 5 | 6 | 7 | /** 8 | * @namespace obvious 9 | */ 10 | namespace obvious 11 | { 12 | 13 | /** 14 | * @class System 15 | * @brief This class encapsulates system specific calls for memory allocation 16 | * @author Stefan May 17 | */ 18 | template 19 | class System 20 | { 21 | public: 22 | /** 23 | * Allocation of 2D arrays 24 | * @param rows number of rows 25 | * @param cols number of columns 26 | * @param array2D data array 27 | */ 28 | static void allocate (unsigned int rows, unsigned int cols, T** &array2D); 29 | 30 | /** 31 | * Deallocation of 2D arrays. Pointers are set to null. 32 | * @param array2D data array 33 | */ 34 | static void deallocate (T** &array2D); 35 | 36 | /** 37 | * Memcpy two-dimensional array 38 | * @param rows number of rows 39 | * @param cols number of columns 40 | * @param src source array 41 | * @param dst destination array 42 | */ 43 | static void copy (unsigned int rows, unsigned int cols, T** &src, T** &dst); 44 | 45 | /** 46 | * Initialize array with zero values 47 | * @param rows number of rows 48 | * @param cols number of columns 49 | * @param buf array 50 | */ 51 | static void initZero(unsigned int rows, unsigned int cols, T** buf); 52 | 53 | /** 54 | * Allocation of 3D arrays 55 | * @param rows number of rows 56 | * @param cols number of columns 57 | * @param slices number of slices 58 | * @param array3D data array 59 | */ 60 | static void allocate (unsigned int rows, unsigned int cols, unsigned int slices, T*** &array3D); 61 | 62 | /** 63 | * Deallocation of 3D arrays. Pointers are set to null. 64 | * @param array3D data array 65 | */ 66 | static void deallocate (T*** &array3D); 67 | 68 | /** 69 | * Memcpy three-dimensional array 70 | * @param rows number of rows 71 | * @param cols number of columns 72 | * @param slices number of slices 73 | * @param src source array 74 | * @param dst destination array 75 | */ 76 | static void copy (unsigned int rows, unsigned int cols, unsigned int slices, T*** &src, T*** &dst); 77 | 78 | /** 79 | * Initialize array with zero values 80 | * @param rows number of rows 81 | * @param cols number of columns 82 | * @param slices number of slices 83 | * @param buf array 84 | */ 85 | static void initZero(unsigned int rows, unsigned int cols, unsigned int slices, T*** buf); 86 | }; 87 | 88 | #include "System.inl" 89 | 90 | } 91 | 92 | #endif /*SYSTEM_H_*/ 93 | -------------------------------------------------------------------------------- /obdevice/ParentDevice3D.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ParentDevice3D.h 3 | * @author Christian Pfitzner 4 | * @date 07.01.2013 5 | * 6 | * 7 | */ 8 | 9 | #ifndef PARENTDEVICE3D_H_ 10 | #define PARENTDEVICE3D_H_ 11 | 12 | #include "obcore/base/Timer.h" 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace obvious { 19 | 20 | class ParentDevice3D 21 | { 22 | public: 23 | /** 24 | * Standard constructor 25 | */ 26 | ParentDevice3D(unsigned int cols = 0, unsigned int rows = 0); 27 | 28 | /** 29 | * Default destructor 30 | */ 31 | virtual ~ParentDevice3D(void) = 0; 32 | /** 33 | * Get number of rows of images 34 | * @return rows 35 | */ 36 | unsigned int getRows() const { return _rows; } 37 | /** 38 | * Get number of columns of images 39 | * @return columns 40 | */ 41 | unsigned int getCols() const { return _cols; } 42 | /** 43 | * Accessor to pointer of coordinate data 44 | * @return pointer to coordinate buffer (layout x1y1z1x2...) 45 | */ 46 | double* getCoords() const { return _coords; } 47 | /** 48 | * Accessor to mask of valid points 49 | * @return pointer to mask 50 | */ 51 | bool* getMask() const { return _mask; } 52 | /** 53 | * Get pointer to z-Buffer 54 | */ 55 | double* getZ() const { return _z; } 56 | /** 57 | * Accessor to pointer of color data 58 | * @return pointer to color buffer (layout r1g1b1r2...) 59 | */ 60 | unsigned char* getRGB() { return _rgb; } 61 | /** 62 | * Function to get the frame rate of sensor 63 | * @return frame rate in pictures per second 64 | */ 65 | float getFrameRate(void) const { return _frameRate; } 66 | /** 67 | * Start serializing the data stream to file 68 | * @param filename name of file 69 | */ 70 | void startRecording(char* filename); 71 | /** 72 | * Stop previously started recording 73 | */ 74 | void stopRecording(void); 75 | protected: 76 | virtual void record(void) = 0; 77 | /** 78 | * Function to estimate frame rate of grabbing 79 | */ 80 | void estimateFrameRate(void); 81 | 82 | unsigned int _rows; 83 | unsigned int _cols; 84 | double* _coords; 85 | double* _z; 86 | bool* _mask; 87 | unsigned char* _rgb; 88 | 89 | Timer _timer; ///< timer for estimation of frame rate 90 | float _frameRate; ///< frame rate of grabbing 91 | 92 | bool _record; 93 | std::ofstream _recfile; 94 | }; 95 | 96 | }; // namespace 97 | 98 | 99 | 100 | #endif /* PARENTDEVICE3D_H_ */ 101 | -------------------------------------------------------------------------------- /obvision/registration/icp/PointToPointEstimator3D.h: -------------------------------------------------------------------------------- 1 | #ifndef POINTTOPOINTESTIMATOR3D_H_ 2 | #define POINTTOPOINTESTIMATOR3D_H_ 3 | 4 | #include "obvision/registration/icp/IRigidEstimator.h" 5 | 6 | namespace obvious 7 | { 8 | 9 | /** 10 | * @class PointToPointEstimator3D 11 | * @brief An estimator for registering points clouds into one common coordinate system. This estimator is based on singular value decomposition (POINTTOPOINT). 12 | * @author Stefan May 13 | */ 14 | class PointToPointEstimator3D : public IRigidEstimator 15 | { 16 | public: 17 | /** 18 | * Default constructor 19 | */ 20 | PointToPointEstimator3D(); 21 | 22 | /** 23 | * Destructor 24 | */ 25 | ~PointToPointEstimator3D(); 26 | 27 | /** 28 | * Setting internal pointer to model array. The model is seen as the ground truth against which the scene has to be registered. 29 | * @param model Pointer to 3 dimensional model array 30 | */ 31 | virtual void setModel(double** model, unsigned int size, double** normals=NULL); 32 | 33 | /** 34 | * Setting internal pointer to scene array. (See commend for setModel) 35 | * @param scene Pointer to 3 dimensional scene array 36 | */ 37 | virtual void setScene(double** scene, unsigned int size, double** normals=NULL); 38 | 39 | /** 40 | * Setting assigned point pairs. You can use a pair assigner for this purpose. The deviation, i.e. the mean distance, between those pairs is also determined within this method. 41 | * @param pairs Vector of pairs of indices. Each index pair references a scene and a model point. 42 | */ 43 | virtual void setPairs(std::vector* pairs); 44 | 45 | /** 46 | * Access the root mean square error that has been calculated by the setPairs method. 47 | * @return RMS error 48 | */ 49 | virtual double getRMS(); 50 | 51 | virtual unsigned int getIterations(void); 52 | 53 | /** 54 | * Determine the transformation matrix that registers the scene to the model. 55 | * @param T transformation matrix as return parameter 56 | */ 57 | virtual void estimateTransformation(Matrix* T); 58 | 59 | private: 60 | 61 | /** 62 | * Centroid of model 63 | */ 64 | double _cm[3]; 65 | 66 | /** 67 | * Model 68 | */ 69 | double** _model; 70 | 71 | /** 72 | * Centroid of scene 73 | */ 74 | double _cs[3]; 75 | 76 | /** 77 | * Scene 78 | */ 79 | double** _scene; 80 | 81 | /** 82 | * Index pairs 83 | */ 84 | std::vector* _pairs; 85 | 86 | /** 87 | * Root mean square error 88 | */ 89 | double _rms; 90 | 91 | unsigned int _iterations; 92 | 93 | }; 94 | 95 | } 96 | 97 | #endif /*POINTTOPOINTESTIMATOR3D_H_*/ 98 | --------------------------------------------------------------------------------