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()) {}
