├── .gitignore ├── Makefile ├── README.md ├── main.cpp ├── pcl_common.cpp ├── pcl_common.h ├── pcl_difference.cpp ├── pcl_intersection.cpp ├── pcl_jaccard_similarity.cpp ├── pcl_make_scanlog.cpp ├── pcl_remove_dynamic_obstacles.cpp ├── pcl_symmetric_difference.cpp ├── pcl_union.cpp └── pcl_union_fast.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.d 3 | *~ 4 | pcl_difference 5 | pcl_intersection 6 | pcl_symmetric_difference 7 | pcl_union 8 | pcl_union_fast 9 | pcl_make_scanlog 10 | pcl_remove_dynamic_obstacles 11 | pcl_jaccard_similarity 12 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | PCL_MODULES=common filters io kdtree sample_consensus 2 | PCL_VERSION=1.7 3 | PCL_CFLAGS = $(shell sh -c 'for i in $(PCL_MODULES); do pkg-config pcl_$${i}-$(PCL_VERSION) --cflags; done') 4 | PCL_LDLIBS = $(shell sh -c 'for i in $(PCL_MODULES); do pkg-config pcl_$${i}-$(PCL_VERSION) --libs; done') 5 | 6 | CFLAGS := -std=c++03 -O0 -ggdb -I/opt/local/include $(PCL_CFLAGS) $(OPT_CFLAGS) 7 | LDLIBS := -lm -lstdc++ -L/opt/local/lib $(PCL_LDLIBS) 8 | 9 | ifeq ($(OS),Windows_NT) 10 | CFLAGS += -DWIN32 11 | ifeq ($(PROCESSOR_ARCHITECTURE),AMD64) 12 | CFLAGS += -DAMD64 13 | endif 14 | ifeq ($(PROCESSOR_ARCHITECTURE),x86) 15 | CFLAGS += -DIA32 16 | endif 17 | else 18 | UNAME_S := $(shell uname -s) 19 | ifeq ($(UNAME_S),Linux) 20 | CFLAGS += -DLINUX 21 | UNAME_P := $(shell uname -p) 22 | ifeq ($(UNAME_P),x86_64) 23 | CFLAGS += -DAMD64 24 | endif 25 | ifneq ($(filter %86,$(UNAME_P)),) 26 | CFLAGS += -DIA32 27 | endif 28 | ifneq ($(filter arm%,$(UNAME_P)),) 29 | CFLAGS += -DARM 30 | endif 31 | LDLIBS += -lboost_system -lboost_thread -lboost_filesystem -lboost_program_options 32 | endif 33 | ifeq ($(UNAME_S),Darwin) 34 | CFLAGS += -DOSX 35 | UNAME_M := $(shell uname -m) 36 | ifeq ($(UNAME_M),x86_64) 37 | CFLAGS += -DAMD64 38 | endif 39 | ifneq ($(filter %86,$(UNAME_M)),) 40 | CFLAGS += -DIA32 41 | endif 42 | LDLIBS += -lboost_system-mt -lboost_thread-mt -lboost_filesystem-mt -lboost_program_options-mt 43 | endif 44 | endif 45 | 46 | CXXFLAGS = $(CFLAGS) 47 | 48 | .PHONY: clean all install 49 | 50 | OBJS = pcl_union.o pcl_intersection.o pcl_difference.o pcl_symmetric_difference.o pcl_common.o main.o pcl_union_fast.o pcl_make_scanlog.o pcl_remove_dynamic_obstacles.o pcl_jaccard_similarity.o 51 | 52 | TARGETS = pcl_union pcl_intersection pcl_difference pcl_symmetric_difference pcl_union_fast pcl_make_scanlog pcl_remove_dynamic_obstacles pcl_jaccard_similarity 53 | 54 | PREFIX = /usr/local 55 | 56 | all: $(TARGETS) 57 | 58 | install: $(TARGETS) 59 | for i in $(TARGETS); do cp -v $$i $(PREFIX)/bin/; done 60 | 61 | pcl_union: pcl_union.o pcl_common.o main.o 62 | $(CC) $^ $(LDLIBS) -o $@ 63 | 64 | pcl_intersection: pcl_intersection.o pcl_common.o main.o 65 | $(CC) $^ $(LDLIBS) -o $@ 66 | 67 | pcl_difference: pcl_difference.o pcl_common.o main.o 68 | $(CC) $^ $(LDLIBS) -o $@ 69 | 70 | pcl_symmetric_difference: pcl_symmetric_difference.o pcl_common.o main.o 71 | $(CC) $^ $(LDLIBS) -o $@ 72 | 73 | pcl_union_fast: pcl_union_fast.o pcl_common.o 74 | $(CC) $^ $(LDLIBS) -o $@ 75 | 76 | pcl_make_scanlog: pcl_make_scanlog.o 77 | $(CC) $^ $(LDLIBS) -o $@ 78 | 79 | pcl_remove_dynamic_obstacles: pcl_remove_dynamic_obstacles.o 80 | $(CC) $^ $(LDLIBS) -o $@ 81 | 82 | pcl_jaccard_similarity: pcl_jaccard_similarity.o pcl_common.o 83 | $(CC) $^ $(LDLIBS) -o $@ 84 | 85 | -include $(OBJS:.o=.d) 86 | 87 | %.o: %.cpp 88 | $(CXX) -c $(CXXFLAGS) $*.cpp -o $*.o 89 | $(CXX) -MM $(CXXFLAGS) $*.cpp > $*.d 90 | @cp -f $*.d $*.d.tmp 91 | @sed -e 's/.*://' -e 's/\\$$//' < $*.d.tmp | fmt -1 | \ 92 | sed -e 's/^ *//' -e 's/$$/:/' >> $*.d 93 | @rm -f $*.d.tmp 94 | 95 | clean: 96 | @rm -vf $(OBJS) $(OBJS:.o=.d) pcl_union pcl_intersection pcl_difference 97 | @sh -c 'if ls *.o > /dev/null 2>&1; then echo "error: some *.o files have not been cleaned: $$(ls *.o)"; false; fi' 98 | 99 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pcl-boolean-op 2 | 3 | A set of utilities for performing boolean operations, such as union, intersection, and difference, on 3D point clouds. 4 | 5 | Requires libpcl [http://pointclouds.org/] 6 | 7 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include "pcl_common.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | parse_args(argc, argv); 6 | 7 | pcl::PointCloud tmp; 8 | std::set a, b, c; 9 | std::string namea, nameb, namec; 10 | 11 | bool first_operand = true; 12 | 13 | namea = input_filenames[0]; 14 | load_pcd(namea.c_str(), tmp); 15 | pcl_to_set(tmp, a); 16 | 17 | for(int i = 1; i < input_filenames.size(); i++) 18 | { 19 | if(!first_operand) 20 | { 21 | c.swap(a); 22 | namea = namec; 23 | } 24 | 25 | b.clear(); 26 | c.clear(); 27 | tmp.clear(); 28 | 29 | nameb = input_filenames[i]; 30 | load_pcd(nameb.c_str(), tmp); 31 | pcl_to_set(tmp, b); 32 | 33 | op(a, b, c); 34 | 35 | std::stringstream namec_ss; 36 | namec_ss << "pcl_" << i; 37 | namec = namec_ss.str(); 38 | 39 | if(verbose) 40 | std::cout << namea << " * " << nameb << " = " << namec << " (" << c.size() << " points)" << std::endl; 41 | 42 | first_operand = false; 43 | } 44 | 45 | if(verbose) 46 | std::cout << "result is " << namec << std::endl; 47 | 48 | tmp.clear(); 49 | set_to_pcl(c, tmp); 50 | save_pcd(output_filename.c_str(), tmp); 51 | } 52 | -------------------------------------------------------------------------------- /pcl_common.cpp: -------------------------------------------------------------------------------- 1 | #include "pcl_common.h" 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | bool ignore_read_errors = true; 8 | bool verbose = false; 9 | double tolerance = 1e-9; 10 | std::vector input_filenames; 11 | std::string output_filename; 12 | 13 | void load_pcd(const char *filename, pcl::PointCloud& cloud) 14 | { 15 | if(pcl::io::loadPCDFile(filename, cloud) == -1 && !ignore_read_errors) 16 | { 17 | if(ignore_read_errors) 18 | { 19 | cloud.clear(); 20 | } 21 | else 22 | { 23 | std::cerr << "error: load of " << filename << " failed" << std::endl; 24 | exit(1); 25 | } 26 | } 27 | if(verbose) 28 | std::cout << "info: loaded " << cloud.size() << " points from " << filename << std::endl; 29 | } 30 | 31 | void save_pcd(const char *filename, const pcl::PointCloud& cloud) 32 | { 33 | if(pcl::io::savePCDFile(filename, cloud) == -1) 34 | { 35 | std::cerr << "error: save of " << filename << " failed" << std::endl; 36 | exit(1); 37 | } 38 | if(verbose) 39 | std::cout << "info: saved " << cloud.size() << " points to " << filename << std::endl; 40 | } 41 | 42 | point::point(double x, double y, double z) 43 | { 44 | ix_ = (long)round(x / tolerance); 45 | iy_ = (long)round(y / tolerance); 46 | iz_ = (long)round(z / tolerance); 47 | } 48 | 49 | bool point::operator<(const point& rhs) const 50 | { 51 | if(ix_ == rhs.ix_) 52 | { 53 | if(iy_ == rhs.iy_) 54 | { 55 | return iz_ < rhs.iz_; 56 | } 57 | else 58 | { 59 | return iy_ < rhs.iy_; 60 | } 61 | } 62 | else 63 | { 64 | return ix_ < rhs.ix_; 65 | } 66 | } 67 | 68 | double point::x() const 69 | { 70 | return ix_ * tolerance; 71 | } 72 | 73 | double point::y() const 74 | { 75 | return iy_ * tolerance; 76 | } 77 | 78 | double point::z() const 79 | { 80 | return iz_ * tolerance; 81 | } 82 | 83 | void pcl_to_set(const pcl::PointCloud& cloud, std::set& s) 84 | { 85 | for(int i = 0; i < cloud.size(); i++) 86 | { 87 | point p(cloud[i].x, cloud[i].y, cloud[i].z); 88 | s.insert(p); 89 | } 90 | } 91 | 92 | void set_to_pcl(const std::set& s, pcl::PointCloud& cloud) 93 | { 94 | for(std::set::iterator it = s.begin(); it != s.end(); ++it) 95 | { 96 | pcl::PointXYZ p; 97 | p.x = it->x(); 98 | p.y = it->y(); 99 | p.z = it->z(); 100 | cloud.push_back(p); 101 | } 102 | } 103 | 104 | void parse_args(int argc, char **argv) 105 | { 106 | namespace po = boost::program_options; 107 | std::string usage = "Usage: " + std::string(argv[0]) + " [options] input_1.pcd input_2.pcd ... input_N.pcd result.pcd"; 108 | po::options_description desc(usage + "\nOptions:"); 109 | desc.add_options() 110 | ("tolerance,t", po::value(&tolerance)->default_value(tolerance), "tolerance ( = voxel size)") 111 | ("stop-on-errors,S", "stop when a readnig error occurs") 112 | ("verbose,v", "verbose mode") 113 | ("help,h", "print help") 114 | ; 115 | po::options_description hdesc("Hidden"); 116 | hdesc.add_options() 117 | ("file,f", po::value >(), "file") 118 | ; 119 | po::options_description desc_all; 120 | desc_all.add(desc).add(hdesc); 121 | po::positional_options_description p; 122 | p.add("file", -1); 123 | po::variables_map vmap; 124 | po::store(po::command_line_parser(argc, argv).options(desc_all).positional(p).run(), vmap); 125 | po::notify(vmap); 126 | 127 | if(vmap.count("help")) 128 | { 129 | std::cout << desc << std::endl; 130 | exit(0); 131 | } 132 | 133 | if(vmap.count("stop-on-errors")) 134 | { 135 | ignore_read_errors = false; 136 | } 137 | 138 | if(vmap.count("verbose")) 139 | { 140 | verbose = true; 141 | } 142 | 143 | try 144 | { 145 | std::vector filenames = vmap["file"].as >(); 146 | if(filenames.size() < 3) 147 | { 148 | std::cout << "error: need at least 3 file arguments (got " << filenames.size() << ")" << std::endl << desc << std::endl; 149 | exit(1); 150 | } 151 | input_filenames.resize(filenames.size() - 1); 152 | std::copy(filenames.begin(), filenames.begin() + filenames.size() - 1, input_filenames.begin()); 153 | output_filename = filenames[filenames.size() - 1]; 154 | } 155 | catch(boost::bad_any_cast& ex) 156 | { 157 | std::cout << desc << std::endl; 158 | exit(1); 159 | } 160 | 161 | if(verbose) 162 | { 163 | if(vmap.count("tolerance")) 164 | std::cout << "info: tolerance set to " << tolerance << std::endl; 165 | else 166 | std::cout << "info: using default tolerance of " << tolerance << std::endl; 167 | } 168 | 169 | if(input_filenames.size() < 2) 170 | { 171 | std::cout << "error: need at least 2 input clouds and 1 output cloud" << std::endl << desc << std::endl; 172 | exit(1); 173 | } 174 | } 175 | 176 | -------------------------------------------------------------------------------- /pcl_common.h: -------------------------------------------------------------------------------- 1 | #ifndef PCL_COMMON_H_INCLUDED 2 | #define PCL_COMMON_H_INCLUDED 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | extern bool ignore_read_errors; 10 | extern bool verbose; 11 | extern double tolerance; 12 | extern std::vector input_filenames; 13 | extern std::string output_filename; 14 | 15 | class point 16 | { 17 | private: 18 | long ix_; 19 | long iy_; 20 | long iz_; 21 | public: 22 | point(double x_, double y_, double z_); 23 | bool operator<(const point& rhs) const; 24 | double x() const; 25 | double y() const; 26 | double z() const; 27 | }; 28 | 29 | void load_pcd(const char *filename, pcl::PointCloud& cloud); 30 | 31 | void save_pcd(const char *filename, const pcl::PointCloud& cloud); 32 | 33 | void pcl_to_set(const pcl::PointCloud& cloud, std::set& s); 34 | 35 | void set_to_pcl(const std::set& s, pcl::PointCloud& cloud); 36 | 37 | void parse_args(int argc, char **argv); 38 | 39 | void op(std::set& a, std::set& b, std::set& c); 40 | 41 | #endif // PCL_COMMON_H_INCLUDED 42 | -------------------------------------------------------------------------------- /pcl_difference.cpp: -------------------------------------------------------------------------------- 1 | #include "pcl_common.h" 2 | 3 | void op(std::set& a, std::set& b, std::set& c) 4 | { 5 | set_difference(a.begin(), a.end(), b.begin(), b.end(), std::inserter(c, c.begin())); 6 | } 7 | 8 | -------------------------------------------------------------------------------- /pcl_intersection.cpp: -------------------------------------------------------------------------------- 1 | #include "pcl_common.h" 2 | 3 | void op(std::set& a, std::set& b, std::set& c) 4 | { 5 | set_intersection(a.begin(), a.end(), b.begin(), b.end(), std::inserter(c, c.begin())); 6 | } 7 | 8 | -------------------------------------------------------------------------------- /pcl_jaccard_similarity.cpp: -------------------------------------------------------------------------------- 1 | #define op op_intersection 2 | #include "pcl_intersection.cpp" 3 | #undef op 4 | 5 | #define op op_union 6 | #include "pcl_union.cpp" 7 | #undef op 8 | 9 | void usage2(const char *self) 10 | { 11 | std::cout << "usage: " << self << " [options] a.pcd b.pcd" << std::endl 12 | << "options:" << std::endl 13 | << " --tolerance " << std::endl 14 | << " -t two points are considered equal if:" << std::endl 15 | << " |x1-x2| tmp; 47 | std::set a, b, c_intersection, c_union; 48 | 49 | if(i + 2 != argc) 50 | { 51 | usage2(argv[0]); 52 | exit(1); 53 | } 54 | 55 | tmp.clear(); 56 | load_pcd(argv[i++], tmp); 57 | pcl_to_set(tmp, a); 58 | 59 | tmp.clear(); 60 | load_pcd(argv[i++], tmp); 61 | pcl_to_set(tmp, b); 62 | 63 | op_intersection(a, b, c_intersection); 64 | op_union(a, b, c_union); 65 | 66 | std::cout << "jaccard similarity index: " << c_intersection.size()/double(c_union.size()) << std::endl; 67 | } 68 | -------------------------------------------------------------------------------- /pcl_make_scanlog.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | void usage(const char *self, int exitcode) 9 | { 10 | std::cout << "usage: " << self << " cloud1.pcd ... cloudN.pcd" << std::endl; 11 | exit(exitcode); 12 | } 13 | 14 | void loadPCL(const char *filename, pcl::PointCloud& cloud, Eigen::Vector4f& t, Eigen::Matrix3f R) 15 | { 16 | pcl::PCLPointCloud2 tmp; 17 | Eigen::Quaternionf r; 18 | if(pcl::io::loadPCDFile(filename, tmp, t, r) == -1) 19 | { 20 | std::cerr << "error: load of " << filename << " failed" << std::endl; 21 | exit(1); 22 | } 23 | pcl::fromPCLPointCloud2(tmp, cloud); 24 | r.normalize(); 25 | R = r.toRotationMatrix(); 26 | } 27 | 28 | #define IGNORE_ROTATION 29 | 30 | int main(int argc, char **argv) 31 | { 32 | Eigen::Vector4f last_t; 33 | Eigen::Matrix3f last_R; 34 | 35 | if(argc < 2 || strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0) 36 | usage(argv[0], 1); 37 | 38 | for(int j = 1; j < argc; j++) 39 | { 40 | Eigen::Vector4f t; 41 | Eigen::Matrix3f R; 42 | pcl::PointCloud tmp2; 43 | 44 | loadPCL(argv[j], tmp2, t, R); 45 | 46 | #ifdef IGNORE_ROTATION 47 | std::cout << "NODE " << t(0) << " " << t(1) << " " << t(2) << " 0 0 0" << std::endl; 48 | #else 49 | Eigen::Matrix euler = R.eulerAngles(2, 1, 0); 50 | float yaw = euler(0,0); 51 | float pitch = euler(1,0); 52 | float roll = euler(2,0); 53 | std::cout << "NODE " << t(0) << " " << t(1) << " " << t(2) << " " << roll << " " << pitch << " " << yaw << std::endl; 54 | #endif 55 | 56 | for(size_t i = 0; i < tmp2.size(); i++) 57 | { 58 | Eigen::Vector3f p, t3; 59 | p << tmp2[i].x, tmp2[i].y, tmp2[i].z; 60 | t3 << t.x(), t.y(), t.z(); 61 | #ifdef IGNORE_ROTATION 62 | p = p - t3; 63 | #else 64 | p = R.inverse() * p - t3; 65 | #endif 66 | std::cout << p.x() << " " << p.y() << " " << p.z() << std::endl; 67 | } 68 | 69 | last_t = t; 70 | last_R = R; 71 | } 72 | } 73 | 74 | -------------------------------------------------------------------------------- /pcl_remove_dynamic_obstacles.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #ifndef M_2PI 11 | #define M_2PI (2 * M_PI) 12 | #endif 13 | 14 | bool verbose = false; 15 | int subdivisions = 64; 16 | 17 | template 18 | inline double dist(const PointA& a, const PointB& b) 19 | { 20 | return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2)); 21 | } 22 | 23 | inline size_t flatBucket(const size_t bucket1, const size_t bucket2, const size_t subdivisions) 24 | { 25 | return bucket1 * subdivisions + bucket2; 26 | } 27 | 28 | inline void angleToBucket(double theta, double phi, size_t& bucket1, size_t& bucket2, const size_t subdivisions) 29 | { 30 | // normalize angles between 0,2PI 31 | while(phi < 0) phi += M_2PI; 32 | while(theta < 0) theta += M_PI; 33 | 34 | bucket1 = (size_t)(phi * subdivisions / M_2PI); 35 | bucket2 = (size_t)(theta * subdivisions / M_PI); 36 | } 37 | 38 | inline void bucketToAngle(const size_t bucket1, const size_t bucket2, double& theta, double& phi, const size_t subdivisions) 39 | { 40 | phi = bucket1 * M_2PI / (double)subdivisions; 41 | theta = bucket2 * M_PI / (double)subdivisions; 42 | } 43 | 44 | template 45 | inline void toEuclidean(double r, double theta, double phi, T& x, T& y, T& z, double offsetX = 0, double offsetY = 0, double offsetZ = 0) 46 | { 47 | x = offsetX + r * sin(theta) * cos(phi); 48 | y = offsetY + r * sin(theta) * sin(phi); 49 | z = offsetZ + r * cos(theta); 50 | } 51 | 52 | inline void toSpherical(double x, double y, double z, double& r, double& theta, double& phi, double offsetX = 0, double offsetY = 0, double offsetZ = 0) 53 | { 54 | // radius [0..infty): 55 | r = sqrt(pow(x - offsetX, 2) + pow(y - offsetY, 2) + pow(z - offsetZ, 2)); 56 | // inclination [0..PI]: 57 | theta = acos((z - offsetZ) / r); 58 | // azimuth [0..2PI): 59 | phi = atan2(y - offsetY, x - offsetX); 60 | } 61 | 62 | template 63 | void sphericalPartition(const pcl::PointCloud& input, pcl::PointCloud *output, const Eigen::Vector4f& center, unsigned int subdivisions) 64 | { 65 | // partition according to phi,theta: 66 | for(size_t i = 0; i < input.size(); i++) 67 | { 68 | const PointT& p = input.points[i]; 69 | double r, theta, phi; 70 | toSpherical(p.x, p.y, p.z, r, theta, phi, center.x(), center.y(), center.z()); 71 | size_t bucket1 = 0, bucket2 = 0; 72 | angleToBucket(theta, phi, bucket1, bucket2, subdivisions); 73 | output[flatBucket(bucket1, bucket2, subdivisions)].push_back(p); 74 | } 75 | } 76 | 77 | template 78 | void removeDynamicObstacles(const pcl::PointCloud& s, const pcl::PointCloud& scan, Eigen::Vector4f scan_pos, Eigen::Quaternionf scan_orient, pcl::PointCloud& result, int num_subdivisions) 79 | { 80 | // remove outliers from scan: 81 | pcl::PointCloud scan_f; 82 | pcl::RadiusOutlierRemoval ror; 83 | ror.setInputCloud(scan.makeShared()); 84 | ror.setRadiusSearch(0.1); 85 | ror.setMinNeighborsInRadius(2); 86 | ror.filter(scan_f); 87 | 88 | pcl::PointXYZ scan_pos_p; 89 | scan_pos_p.x = scan_pos.x(); 90 | scan_pos_p.y = scan_pos.y(); 91 | scan_pos_p.z = scan_pos.z(); 92 | 93 | pcl::PointCloud scan_partition[num_subdivisions * num_subdivisions]; 94 | pcl::PointCloud s_partition[num_subdivisions * num_subdivisions]; 95 | 96 | sphericalPartition(scan_f, scan_partition, scan_pos, num_subdivisions); 97 | sphericalPartition(s, s_partition, scan_pos, num_subdivisions); 98 | 99 | for(size_t bucket1 = 0; bucket1 < num_subdivisions; bucket1++) 100 | { 101 | for(size_t bucket2 = 0; bucket2 < num_subdivisions; bucket2++) 102 | { 103 | size_t bucket = flatBucket(bucket1, bucket2, num_subdivisions); 104 | 105 | // find min distance: 106 | double min_dist = INFINITY; 107 | for(size_t i = 0; i < scan_partition[bucket].size(); i++) 108 | min_dist = std::min(min_dist, dist(scan_pos_p, scan_partition[bucket][i])); 109 | if(isinf(min_dist)) 110 | min_dist = 0.0; 111 | 112 | // add points from map that do not fall into deletion volume: 113 | for(size_t j = 0; j < s_partition[bucket].size(); j++) 114 | { 115 | double d = dist(scan_pos_p, s_partition[bucket][j]); 116 | 117 | if(d >= min_dist) 118 | { 119 | result.push_back(s_partition[bucket][j]); 120 | } 121 | } 122 | 123 | // add all points from scan: 124 | for(size_t j = 0; j < scan_partition[bucket].size(); j++) 125 | { 126 | result.push_back(scan_partition[bucket][j]); 127 | } 128 | } 129 | } 130 | } 131 | 132 | int main(int argc, char **argv) 133 | { 134 | namespace po = boost::program_options; 135 | po::options_description desc("Usage: pcl_remove_dynamic_obstacles [options] s.pcd scan.pcd result.pcd\nOptions:"); 136 | desc.add_options() 137 | ("subdivisions,s", po::value(&subdivisions)->default_value(subdivisions), "number of subdivisions") 138 | ("verbose,v", "verbose mode") 139 | ("help,h", "print help") 140 | ; 141 | po::options_description hdesc("Hidden"); 142 | hdesc.add_options() 143 | ("input-s,i", po::value(), "s input file") 144 | ("input-scan,j", po::value(), "scan input file") 145 | ("output,o", po::value(), "output file") 146 | ; 147 | po::options_description desc_all; 148 | desc_all.add(desc).add(hdesc); 149 | po::positional_options_description p; 150 | p.add("input-s", 1); 151 | p.add("input-scan", 1); 152 | p.add("output", 1); 153 | po::variables_map vmap; 154 | po::store(po::command_line_parser(argc, argv).options(desc_all).positional(p).run(), vmap); 155 | po::notify(vmap); 156 | 157 | if(vmap.count("help")) 158 | { 159 | std::cout << desc << std::endl; 160 | exit(0); 161 | } 162 | 163 | if(vmap.count("verbose")) 164 | { 165 | verbose = true; 166 | } 167 | 168 | Eigen::Vector4f t; 169 | Eigen::Quaternionf q; 170 | pcl::PCLPointCloud2 s2, scan2; 171 | pcl::PointCloud s, scan, result; 172 | 173 | std::string s_fn; 174 | std::string scan_fn; 175 | std::string result_fn; 176 | try 177 | { 178 | s_fn = vmap["input-s"].as(); 179 | scan_fn = vmap["input-scan"].as(); 180 | result_fn = vmap["output"].as(); 181 | } 182 | catch(boost::bad_any_cast& ex) 183 | { 184 | std::cout << desc << std::endl; 185 | exit(1); 186 | } 187 | 188 | if(vmap.count("help")) 189 | { 190 | std::cout << desc << std::endl; 191 | exit(0); 192 | } 193 | 194 | if(pcl::io::loadPCDFile(s_fn, s2) == -1) 195 | { 196 | std::cerr << "error: load of " << s_fn << " failed" << std::endl; 197 | return 1; 198 | } 199 | pcl::fromPCLPointCloud2(s2, s); 200 | if(verbose) 201 | std::cout << "info: loaded " << s.size() << " points from " << s_fn << std::endl; 202 | 203 | if(pcl::io::loadPCDFile(scan_fn, scan2, t, q) == -1) 204 | { 205 | std::cerr << "error: load of " << scan_fn << " failed" << std::endl; 206 | return 1; 207 | } 208 | pcl::fromPCLPointCloud2(scan2, scan); 209 | if(verbose) 210 | std::cout << "info: loaded " << scan.size() << " points from " << scan_fn << std::endl; 211 | 212 | removeDynamicObstacles(s, scan, t, q, result, subdivisions); 213 | 214 | if(pcl::io::savePCDFile(result_fn, result) == -1) 215 | { 216 | std::cerr << "error: write of " << result_fn << " failed" << std::endl; 217 | return 1; 218 | } 219 | if(verbose) 220 | std::cout << "info: wrote " << result.size() << " points to " << result_fn << std::endl; 221 | } 222 | 223 | -------------------------------------------------------------------------------- /pcl_symmetric_difference.cpp: -------------------------------------------------------------------------------- 1 | #include "pcl_common.h" 2 | 3 | void op(std::set& a, std::set& b, std::set& c) 4 | { 5 | set_symmetric_difference(a.begin(), a.end(), b.begin(), b.end(), std::inserter(c, c.begin())); 6 | } 7 | 8 | -------------------------------------------------------------------------------- /pcl_union.cpp: -------------------------------------------------------------------------------- 1 | #include "pcl_common.h" 2 | 3 | void op(std::set& a, std::set& b, std::set& c) 4 | { 5 | set_union(a.begin(), a.end(), b.begin(), b.end(), std::inserter(c, c.begin())); 6 | } 7 | 8 | -------------------------------------------------------------------------------- /pcl_union_fast.cpp: -------------------------------------------------------------------------------- 1 | #include "pcl_union.cpp" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | parse_args(argc, argv); 6 | 7 | pcl::PointCloud tmp; 8 | std::vector > pcl; 9 | 10 | int k = 0; 11 | pcl.resize(input_filenames.size()); 12 | for(std::vector::iterator it = input_filenames.begin(); it != input_filenames.end(); ++it) 13 | { 14 | load_pcd(it->c_str(), tmp); 15 | pcl_to_set(tmp, pcl[k++]); 16 | tmp.clear(); 17 | } 18 | 19 | if(verbose) 20 | std::cout << "loaded " << pcl.size() << " point clouds" << std::endl; 21 | 22 | while(pcl.size() > 1) 23 | { 24 | int l2 = (pcl.size() + 1) / 2; 25 | 26 | for(int j = 0; j < l2; j++) 27 | { 28 | std::set c; 29 | if((2*j+1) < pcl.size()) 30 | { 31 | op(pcl[2*j], pcl[2*j+1], c); 32 | } 33 | else 34 | { 35 | pcl[2*j].swap(c); 36 | } 37 | pcl[j].swap(c); 38 | } 39 | pcl.resize(l2); 40 | } 41 | 42 | set_to_pcl(pcl[0], tmp); 43 | save_pcd(output_filename.c_str(), tmp); 44 | } 45 | 46 | --------------------------------------------------------------------------------