polygon_t* polygon_star(point_t* x0, point_t* points, int num_points) { ASSERT(num_points > 2); ASSERT(all_points_are_coplanar(points, num_points)); // Find the plane of the polygon. vector_t normal; compute_normal(points, num_points, &normal); point_t xp; compute_centroid(points, num_points, &xp); sp_func_t* plane = plane_new(&normal, &xp); // Find the angles of the points within the plane, and sort the points // by this angle. point2_t pts[num_points]; for (int i = 0; i < num_points; ++i) plane_project(plane, &points[i], &pts[i]); point2_t xc; plane_project(plane, x0, &xc); polygon2_t* poly2 = polygon2_star(&xc, pts, num_points); // Read off the vertex ordering from the planar polygon. ASSERT(polygon2_num_vertices(poly2) == num_points); int* ordering = polygon2_ordering(poly2); // Create our polygon with the given ordering. polygon_t* poly = polygon_new_with_ordering(points, ordering, num_points); // Clean up. poly2 = NULL; plane = NULL; return poly; }
void face_scan(const char *line, struct face *f, struct vertex *v, int vertex_count) { int n, i; int index[3]; /* TODO face format is more complex than this */ n = sscanf(line, "f %d %d %d", &index[0], &index[1], &index[2]); if (n == 3) { for (i = 0; i < 3; i++) { if ((index[i] > vertex_count) || (index[i] < 1)) { fprintf(stderr, "bad face line " "(vertex index %d out of bounds):" "\n%s\n", index[i], line); } else { f->v[i].x = v[index[i]-1].x; f->v[i].y = v[index[i]-1].y; f->v[i].z = v[index[i]-1].z; f->v[i].w = 1.0; } } compute_normal(f); for (i = 0; i < 3; i++) compute_centroid(f); } else { fprintf(stderr, "bad face line (needs 3 vertices):\n%s\n", line); } }
polygon_t* polygon_giftwrap(point_t* points, int num_points) { ASSERT(num_points > 2); ASSERT(all_points_are_coplanar(points, num_points)); // Find the plane of the polygon. vector_t normal; compute_normal(points, num_points, &normal); point_t x0; compute_centroid(points, num_points, &x0); sp_func_t* plane = plane_new(&normal, &x0); // Do the gift-wrapping in 2D. point2_t pts[num_points]; for (int i = 0; i < num_points; ++i) plane_project(plane, &points[i], &pts[i]); polygon2_t* poly2 = polygon2_giftwrap(pts, num_points); #if 0 // Re-embed the resulting vertices in 3D. int num_vertices = polygon2_num_vertices(poly2); point_t vertices[num_vertices]; int pos = 0, offset = 0; point2_t* vtx; while (polygon2_next_vertex(poly2, &pos, &vtx)) plane_embed(plane, vtx, &vertices[offset++]); #endif // Read off the vertex ordering from the planar polygon. Note that // not all of the vertices will be used in general. int num_p2_points = polygon2_num_vertices(poly2); int* ordering = polygon2_ordering(poly2); // Create our polygon with the given ordering. polygon_t* poly = NULL; if (num_p2_points < num_points) { point_t p2_points[num_p2_points]; for (int i = 0; i < num_p2_points; ++i) point_copy(&p2_points[i], &points[ordering[i]]); poly = polygon_new(p2_points, num_p2_points); } else poly = polygon_new_with_ordering(points, ordering, num_points); // Clean up. poly2 = NULL; plane = NULL; return poly; }
void polygon_clip(polygon_t* poly, polygon_t* other) { // Do the clipping in 2D. point2_t pts[poly->num_vertices]; for (int i = 0; i < poly->num_vertices; ++i) { int I = poly->ordering[i]; plane_project(poly->plane, &poly->vertices[I], &pts[I]); } polygon2_t* poly2 = polygon2_new(pts, poly->num_vertices); point2_t other_pts[other->num_vertices]; for (int i = 0; i < other->num_vertices; ++i) { int I = poly->ordering[i]; plane_project(other->plane, &other->vertices[I], &other_pts[I]); } polygon2_t* other2 = polygon2_new(other_pts, other->num_vertices); polygon2_clip(poly2, other2); // Now re-embed the vertices. int num_vertices = polygon2_num_vertices(poly2); if (poly->num_vertices < num_vertices) poly->vertices = polymec_realloc(poly->vertices, sizeof(point_t)*num_vertices); poly->num_vertices = num_vertices; int pos = 0, offset = 0; point2_t* vtx; int* ordering2 = polygon2_ordering(poly2); while (polygon2_next_vertex(poly2, &pos, &vtx)) { int I = ordering2[offset++]; plane_embed(poly->plane, vtx, &poly->vertices[I]); } // Recompute geometry (plane remains unchanged). compute_centroid(poly->vertices, poly->num_vertices, &poly->x0); // Clean up. poly2 = NULL; other2 = NULL; }
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { /* declare variables */ mwSize width, height, nsegms; mwSize *sizes; bool *segms; float *o1, *o2; /* check argument */ if (nargin<1) { mexErrMsgTxt("One input argument required ( the segments in row * column * num_segments form )"); } if (nargout>2) { mexErrMsgTxt("Too many output arguments"); } /* sizes */ sizes = (mwSize *)mxGetDimensions(in[0]); height = sizes[0]; width = sizes[1]; nsegms = sizes[2]; if (!mxIsLogical(in[0]) || mxGetNumberOfDimensions(in[0]) != 3) { mexErrMsgTxt("Usage: segms must be a 3-dimensional logical matrix"); } segms = (bool *) mxGetData(in[0]); out[0] = mxCreateNumericMatrix(nsegms, 1 ,mxSINGLE_CLASS, mxREAL); out[1] = mxCreateNumericMatrix(nsegms, 1 ,mxSINGLE_CLASS, mxREAL); if (out[0]==NULL || out[1]==NULL) { mexErrMsgTxt("Not enough memory for the output matrix"); } o1 = (float *) mxGetPr(out[0]); o2 = (float *) mxGetPr(out[1]); compute_centroid(segms, height, width, nsegms, o1, o2); }
// I need to change the parameters to account for the // transformation matrices. I need to save the best TM // so I can combine it with the icp TM. // // Does kdtree search on each rotation of 45 degrees around // the central point of the moved_cloud Eigen::Matrix4f kdtree_search(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr moved_cloud, float radius) { pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; // Total number of nearest neighbors int num_matches = 0; // Return the cloud with the most number of nearest neighbors // within a given radius of the searchPoint int max_neighbors = 0; pcl::PointCloud<pcl::PointXYZ>::Ptr best_fit_cloud ; Eigen::Matrix4f best_fit_transform = Eigen::Matrix4f::Identity(); Eigen::Vector4f centroid1 = compute_centroid(*moved_cloud); cout << "centroid of source cloud - " << centroid1(0) << ", " << centroid1(1) << ", " << centroid1(2) << endl; // Executing the transformation pcl::PointCloud<pcl::PointXYZ>::Ptr rotated_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); Eigen::Matrix4f transform_2 = Eigen::Matrix4f::Identity(); for (int i = 0; i < 8; i++) { float theta = 45 * i + 45; transform_1 (0,0) = cos (theta); transform_1 (0,2) = -sin (theta); transform_1 (2,0) = sin (theta); transform_1 (2,2) = cos (theta); cout << "cloud with " << theta << " degrees of rotation" << endl; pcl::transformPointCloud (*moved_cloud, *rotated_cloud, transform_1); // Probably need to compute centroid of the new transformed cloud // because the transformation seems to translate it Eigen::Vector4f centroid2 = compute_centroid(*rotated_cloud); cout << "centroid of rotated cloud - " << centroid2(0) << ", " << centroid2(1) << ", " << centroid2(2) << endl; float distance_x = centroid1(0) - centroid2(0); float distance_y = centroid1(1) - centroid2(1); float distance_z = centroid1(2) - centroid2(2); cout << "distance between centroids: (" << distance_x << ", " << distance_y << ", " << distance_z << ")" << endl; transform_2 (0,3) = (distance_x); transform_2 (1,3) = (distance_y); transform_2 (2,3) = (distance_z); pcl::transformPointCloud (*rotated_cloud, *transformed_cloud, transform_2); // Rotate the cloud by 45 degrees each time // May want to add some random translation as well. // This would correspond to doing kdtree search on a number of // clouds that are presumably close to the target point cloud. kdtree.setInputCloud(transformed_cloud); // Run the kdtree search on every 10th point of the moved_cloud // Increase this number to speed up the search // Test with different increments of i to see effect on speed for (int j = 0; j < (*moved_cloud).points.size(); j += 10) { pcl::PointXYZ searchPoint = moved_cloud->points[j]; int num_neighbors = kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance); num_matches += num_neighbors; cout << "performed kdtree nearest neighbor search, found " << num_neighbors << " within " << radius << " radius" << endl; } cout << "num_matches = " << num_matches << endl; cout << "max_neighbors = " << max_neighbors << endl; if (num_matches > max_neighbors) { max_neighbors = num_matches; best_fit_cloud = transformed_cloud; // are these transforms relative? or absolute? // this currently calculates relative // should be transform_2 * transform_1 if absolute best_fit_transform = transform_2 * transform_1; } num_matches = 0; printf( "\nPoint cloud colors : white = original point cloud\n" " red = transformed point cloud\n" " blue = moved point cloud\n" " green = rotated point cloud\n"); pcl::visualization::PCLVisualizer viewer ("Matrix transformation example"); // Define R,G,B colors for the point cloud // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255); // white // We add the point cloud to the viewer and pass the color handler // viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rotated_cloud_color_handler (rotated_cloud, 20, 245, 20); // green viewer.addPointCloud (rotated_cloud, rotated_cloud_color_handler, "rotated_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> moved_cloud_color_handler (moved_cloud, 20, 230, 230); // blue viewer.addPointCloud (moved_cloud, moved_cloud_color_handler, "moved_cloud"); viewer.addCoordinateSystem (1.0, 0); viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey // viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "rotated_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "moved_cloud"); //viewer.setPosition(800, 400); // Setting visualiser window position while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed viewer.spinOnce (); } } // check whether the translations are relative or absolute pcl::PointCloud<pcl::PointXYZ>::Ptr best_transform_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::transformPointCloud (*moved_cloud, *best_transform_cloud, best_fit_transform); pcl::visualization::PCLVisualizer viewer ("Matrix transformation example"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_transform_cloud_color_handler (best_transform_cloud, 245, 20, 20); // red viewer.addPointCloud (best_transform_cloud, best_transform_cloud_color_handler, "best_transform_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_fit_cloud_color_handler (best_fit_cloud, 20, 245, 20); // green viewer.addPointCloud (best_transform_cloud, best_fit_cloud_color_handler, "best_fit_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_cloud_color_handler (input_cloud, 255, 255, 255); // white viewer.addPointCloud (input_cloud, input_cloud_color_handler, "input_cloud"); viewer.addCoordinateSystem (1.0, 0); viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_transform_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_fit_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input_cloud"); while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed viewer.spinOnce (); } return best_fit_transform; }
static void polygon_compute_plane(polygon_t* poly) { compute_normal(poly->vertices, poly->num_vertices, &poly->normal); compute_centroid(poly->vertices, poly->num_vertices, &poly->x0); poly->plane = plane_new(&poly->normal, &poly->x0); }
// for the small number of points usually needed to initialise an atom, // a copy constructor for gvec_list would probably do. Tetrahedron(gvec_list points) : points(points) { if (points.size() != 4) { throw std::invalid_argument("A tetrahedron has four points."); } compute_centroid(); };
IMPCORE_BEGIN_INTERNAL_NAMESPACE Referential::Referential(Model* m, ParticleIndex pi) : m_(m), pi_(pi), centroid_(compute_centroid()), base_(compute_base()), q_(compute_quaternion()) {}