Implemented mesh reader, occupancy map, starting integrating into dsl…

…_grid3d

.gitignore

@@ -0,0 +1 @@
+*.swp

CMakeLists.txt

@@ -0,0 +1,184 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dsl_grid3d)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  tf
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Eigen REQUIRED)
+find_package(OpenMP REQUIRED)
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES dslviz
+  CATKIN_DEPENDS roscpp rospy std_msgs
+  DEPENDS eigen system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  include
+  ${CMAKE_CURRENT_SOURCE_DIR}/extern/trimesh/include
+  ${catkin_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+)
+
+link_directories(${CMAKE_CURRENT_SOURCE_DIR}/extern/gluit/bin)
+link_directories(${CMAKE_CURRENT_SOURCE_DIR}/extern/trimesh/bin)
+#get_property(output DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY LINK_DIRECTORIES)
+#message(STATUS ${output})
+
+## Declare a cpp library
+add_library(dsl_grid3d
+  src/${PROJECT_NAME}/mesh_utility.cpp
+  src/${PROJECT_NAME}/dsl_grid3d.cpp
+  src/${PROJECT_NAME}/occupancy_grid.cpp
+)
+
+target_link_libraries(dsl_grid3d
+  libdsl.so
+)
+
+## Declare a cpp executable
+add_executable(dsl_grid3d_node src/${PROJECT_NAME}/dsl_grid3d_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(dslviz_node dslviz_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(dsl_grid3d_node
+  dsl_grid3d
+  libtrimesh.a
+  libgluit.a
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS dslviz dslviz_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_dslviz.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

extern/trimesh/include/trimesh2/Box.h

@@ -0,0 +1,167 @@
+#ifndef BOX_H
+#define BOX_H
+/*
+Szymon Rusinkiewicz
+Princeton University
+
+Box.h
+Templated axis-aligned bounding boxes - meant to be used with Vec.h
+*/
+
+#include "Vec.h"
+#include "strutil.h"
+#include <iostream>
+#include <fstream>
+
+
+namespace trimesh {
+
+template <size_t D, class T = float>
+class Box {
+private:
+	typedef Vec<D,T> Point;
+
+public:
+	Point min, max;
+	bool valid;
+
+	// Construct as empty
+	Box() : valid(false)
+		{}
+
+	// Construct from a single point
+	Box(const Point &p) : min(p), max(p), valid(true)
+		{}
+
+	// Mark invalid
+	void clear()
+		{ valid = false; }
+
+	// Return center point, (vector) diagonal, and (scalar) radius
+	Point center() const
+	{
+		if (valid)
+			return T(0.5) * (min+max);
+		else
+			return Vec<D,T>();
+	}
+	Point size() const
+	{
+		if (valid)
+			return max - min;
+		else
+			return Vec<D,T>();
+	}
+	T radius() const
+	{
+		if (valid)
+			return T(0.5) * dist(min, max);
+		else
+			return T(0);
+	}
+
+	// Grow a bounding box to encompass a point
+	Box<D,T> &operator += (const Point &p)
+	{
+		if (valid) {
+			min.min(p);
+			max.max(p);
+		} else {
+			min = p;
+			max = p;
+			valid = true;
+		}
+		return *this;
+	}
+	Box<D,T> &operator += (const Box<D,T> &b)
+	{
+		if (valid) {
+			min.min(b.min);
+			max.max(b.max);
+		} else {
+			min = b.min;
+			max = b.max;
+			valid = true;
+		}
+		return *this;
+	}
+
+	friend const Box<D,T> operator + (const Box<D,T> &b, const Point &p)
+		{ return Box<D,T>(b) += p; }
+	friend const Box<D,T> operator + (const Point &p, const Box<D,T> &b)
+		{ return Box<D,T>(b) += p; }
+	friend const Box<D,T> operator + (const Box<D,T> &b1, const Box<D,T> &b2)
+		{ return Box<D,T>(b1) += b2; }
+
+	// Read a Box from a file.
+	bool read(const ::std::string &filename)
+	{
+		using namespace ::std;
+		fstream f(filename.c_str());
+		Box<D,T> B;
+		f >> B;
+		f.close();
+		if (f.good()) {
+			*this = B;
+			return true;
+		}
+		return false;
+	}
+
+	// Write a Box to a file
+	bool write(const ::std::string &filename) const
+	{
+		using namespace ::std;
+		ofstream f(filename.c_str());
+		f << *this;
+		f.close();
+		return f.good();
+	}
+
+	// iostream operators
+	friend ::std::ostream &operator << (::std::ostream &os, const Box<D,T> &b)
+	{
+		using namespace ::std;
+		const size_t n = b.min.size();
+		for (size_t i = 0; i < n-1; i++)
+			os << b.min[i] << " ";
+		os << b.min[n-1] << endl;
+		for (size_t i = 0; i < n-1; i++)
+			os << b.max[i] << " ";
+		os << b.max[n-1] << endl;
+		return os;
+	}
+	friend ::std::istream &operator >> (::std::istream &is, Box<D,T> &b)
+	{
+		using namespace ::std;
+		const size_t n = b.min.size();
+		for (size_t i = 0; i < n; i++)
+			is >> b.min[i];
+		for (size_t i = 0; i < n; i++)
+			is >> b.max[i];
+		for (size_t i = 0; i < n; i++)
+			if (b.min[i] > b.max[i])
+				swap(b.min[i], b.max[i]);
+		b.valid = is.good();
+		return is;
+	}
+};
+
+typedef Box<3,float> box;
+typedef Box<2,float> box2;
+typedef Box<3,float> box3;
+typedef Box<4,float> box4;
+typedef Box<2,int> ibox2;
+typedef Box<3,int> ibox3;
+typedef Box<4,int> ibox4;
+
+
+// Generate a .bbox filename from an input (scan) filename
+static inline ::std::string bboxname(const ::std::string &filename)
+{
+	return replace_ext(filename, "bbox");
+}
+
+}; // namespace trimesh
+
+#endif