예제 #1
0
파일: atlaas.cpp 프로젝트: pierriko/atlaas
/**
 * Merge point cloud in the internal model
 * with the sensor to world transformation,
 * and slide, save, load tiles.
 *
 * @param cloud: point cloud in the sensor frame
 * @param transformation: sensor to world transformation
 */
void atlaas::merge(points& cloud, const matrix& transformation,
        const covmat& covariance, bool dump) {
    if (cloud.size() < 1)
        return; // pcl writeBinaryCompressed crash with empty cloud
    if (dump) {
        save_inc(cloud, transformation);
        if (reprocess_in_progress)
            return;
    }

    sensor_xy = matrix_to_point(transformation);
    // slide map while needed
    do_slide();
    // use dynamic merge
    // clear the dynamic map (zeros)
    cell_info_t zeros{}; // value-initialization w/empty initializer
    std::fill(dyninter.begin(), dyninter.end(), zeros);
    // transform the cloud from sensor to custom frame
    transform(cloud, transformation);
    // merge the point-cloud
    rasterize(cloud, dyninter);

    // merge the dynamic atlaas with internal data
    merge();
}
예제 #2
0
파일: Geo.cpp 프로젝트: moretimes/template
void convex_hull(points &a, points &res) {
	res.resize(2 * a.size() + 10);
	sort(a.begin(), a.end(), cmpxy);
	a.erase(unique(a.begin(), a.end()), a.end());
	int m = 0;
	for (int i = 0; i < (int)a.size(); i ++) {
		while(m > 1 && dcmp(det(res[m - 1] - res[m - 2], a[i] - res[m - 2])) <= 0) --m;
		res[m++] = a[i];
	}
	int k = m;
	for (int i = (int)a.size() - 2;  i >= 0; i --) {
		while(m > k && dcmp(det(res[m - 1] - res[m - 2], a[i] - res[m - 2])) <= 0) --m;
		res[m++] = a[i];
	}
	res.resize(m);
	//if (a.size() > 1) res.resize(m - 1);
}
예제 #3
0
파일: Geo.cpp 프로젝트: moretimes/template
void cut(points &p, point b, point a, points &res) {
	res.clear();
	int n = p.size();
	for (int i = 0; i < n; i ++) {
		point c = p[i];
		point d = p[next(i)];
		if (dcmp(det(b - a, c - a)) >= 0) res.push_back(c);
		if (dcmp(det(b - a, c - d)) != 0) {
			point cp = interpoint(line(a, b), line(c, d));
			if (onseg(cp, c, d)) res.push_back(cp);
		}
	}
}
예제 #4
0
points convexHull (points a)
{
	points up,dw;
	sort (a.begin(), a.end());
	for (int i=0;i<a.size();i++)
	{
		while (up.size()>=2 && ccw(*++up.rbegin(),*up.rbegin(),a[i])>=0) up.pop_back();
		while (dw.size()>=2 && ccw(*++dw.rbegin(),*dw.rbegin(),a[i])<=0) dw.pop_back();
		up.emplace_back(a[i]);
		dw.emplace_back(a[i]);
	}
	up.insert (up.end(), ++dw.rbegin(), --dw.rend());
	return up;
}
예제 #5
0
int main(int argc, char *argv[])
{
	if (argc != 4)
	{
		std::cout << usage << std::endl;
		return -1;
	}

	std::string line;
	std::ifstream input(argv[1], std::ifstream::in);

	std::getline(input, line);
	std::istringstream seiss(line);
	double sx, sy, ex, ey;
	seiss >> sx >> sy >> ex >> ey;
	point start(sx, sy), end(ex, ey);

	std::getline(input, line);
	std::istringstream niss(line);
	int count = 0;
	niss >> count;
	for (int i = 0; i < count; i++)
	{
		std::getline(input, line);
		std::istringstream iss(line);
		double x, y;
		iss >> x >> y;
		polygon_points.push_back(point(x, y));
	}

	int vertices_count = polygon_points.size();
	int triangles_count = polygon_points.size() - 2;

	double (*vertices)[2] = new double[vertices_count + 1][2];
	int (*triangles)[3] = new int[triangles_count][3];

	for (int i = 1; i <= polygon_points.size(); i++)
	{
		vertices[i][0] = polygon_points[i - 1].x;
		vertices[i][1] = polygon_points[i - 1].y;
	}
	int ncontours = 1;
	int cntr[1] = {polygon_points.size()};
	triangulate_polygon(ncontours, cntr, vertices, triangles);

	for (int i = 0; i < triangles_count; i++)
	{
		triangles[i][0]--;
		triangles[i][1]--;
		triangles[i][2]--;
	}
	std::ofstream output(argv[2], std::ofstream::out);
	int start_tri = locate_point(triangles, triangles_count, start);
	int end_tri = locate_point(triangles, triangles_count, end);
	bool distance = false;
	if (strcmp(argv[3], "path") == 0)
		distance = false;
	else if (strcmp(argv[3], "distance") == 0)
		distance = true;
	if (start_tri == end_tri)
	{
		if (distance)
			output << (int)ceil(dist(start, end)) << std::endl;
		else
		{
			output << "s" << std::endl;
			output << "e" << std::endl;
		}
	}
	else
	{
		std::vector<int> triangles_list = dfs(triangles, triangles_count, start_tri, end_tri);
		std::vector<diagnal> d_list = diagnals_list(triangles, triangles_list);
		path shortest_path = simple_stupid_funnel(d_list, start, end);

		if (distance)
		{
			double total_dist = 0;
			for (int i = 0; i < shortest_path.size(); i++)
			{
				point last;
				point current = polygon_points[shortest_path[i]];
				if (i == 0)
					last = start;
				else
					last = polygon_points[shortest_path[i - 1]];
				total_dist += dist(last, current);
			}
			if (shortest_path.size() > 0)
				total_dist += dist(polygon_points[shortest_path[shortest_path.size() - 1]], end);
			else
				total_dist += dist(start, end);
			output << (int)ceil(total_dist) << std::endl;
		}
		else
		{
			output << "s" << std::endl;
			for (int i = 0; i < shortest_path.size(); i++)
				output << shortest_path[i] << std::endl;
			output << "e" << std::endl;
		}
	}

	delete[] vertices;
	delete[] triangles;
	return 0;
}
예제 #6
0
파일: Geo.cpp 프로젝트: moretimes/template
double area(points &p) {
	double z = 0;
	for (int i = 0; i < (int)p.size() - 1; i ++)
		z += det(p[i] - p[0], p[i + 1] - p[0]);
	return fabs(z) / 2;
}