TEST_F(test_map, map_image_corners) { SKIP_IF_FAST vector<Point> corners; // select points close to corners, avoid NaNs int margin = 160; corners.push_back(Point(margin, margin)); corners.push_back(Point(img.cols - margin, margin)); corners.push_back(Point(img.cols - margin, img.rows - margin)); corners.push_back(Point(margin, img.rows - margin)); PointCloudT corners3d; mapToCloud(corners3d, corners, img, cloud); EXPECT_EQ(corners.size(), corners3d.size()); EXPECT_EQ(4, corners.size()); EXPECT_EQ(4, corners3d.size()); for (PointCloudT::iterator it = corners3d.begin(), end = corners3d.end(); it != end; it++) { cout << boost::format("%d: %8f,%8f,%8f") % (it - corners3d.begin()) % (*it).x % (*it).y % (*it).z << endl; } // show the whole stuff PCLVisualizer vis; addPose(vis, PoseRT()); addMarkerPolygon3d(vis, corners3d); vis.addPointCloud(cloud); vis.spin(); }
TEST_F(test_map, map_markers) { SKIP_IF_FAST vector<Point> red2d; vector<Point> green2d; vector<Point> yellow2d; vector<Point> blue2d; red2d.push_back(Point(727, 269)); red2d.push_back(Point(716, 448)); red2d.push_back(Point(806, 449)); red2d.push_back(Point(834, 268)); green2d.push_back(Point(336, 195)); green2d.push_back(Point(1157, 198)); yellow2d.push_back(Point(338, 183)); yellow2d.push_back(Point(1154, 189)); blue2d.push_back(Point(697, 86)); blue2d.push_back(Point(371, 92)); PointCloudT red3d; PointCloudT green3d; PointCloudT yellow3d; PointCloudT blue3d; mapToCloud(red3d, red2d, markerImg, cloud); mapToCloud(green3d, green2d, markerImg, cloud); mapToCloud(yellow3d, yellow2d, markerImg, cloud); mapToCloud(blue3d, blue2d, markerImg, cloud); /* PointCloudT red3d_hscaled; PointCloudT green3d_hscaled; PointCloudT yellow3d_hscaled; PointCloudT blue3d_hscaled; mapToCloud(red3d_hscaled, red2d, markerImg, cloud, false); mapToCloud(green3d_hscaled, green2d, markerImg, cloud, false); mapToCloud(yellow3d_hscaled, yellow2d, markerImg, cloud, false); mapToCloud(blue3d_hscaled, blue2d, markerImg, cloud, false); EXPECT_EQ(red2d.size(), red3d_hscaled.size()); EXPECT_EQ(green2d.size(), green3d_hscaled.size()); EXPECT_EQ(yellow2d.size(), yellow3d_hscaled.size()); EXPECT_EQ(blue2d.size(), blue3d_hscaled.size()); */ EXPECT_EQ(640, cloud.width); EXPECT_EQ(480, cloud.height); imshow("MapMarkers", markerImg); waitKey(-1); // TODO: PCLVisualizer segfaults for no reason... #ifdef ENABLE_PCLVIS // show the whole stuff PCLVisualizer vis; addPose(vis, PoseRT()); addMarkerPolygon3d(vis, red3d, 255, 0, 0, "red"); addMarkerPolygon3d(vis, green3d, 0, 216, 0, "green"); addMarkerPolygon3d(vis, yellow3d, 255, 216, 0, "yellow"); addMarkerPolygon3d(vis, blue3d, 0, 66, 255, "blue"); /* addMarkerPolygon3d(vis, red3d_hscaled, 204, 153, 153, "red_hscaled"); addMarkerPolygon3d(vis, green3d_hscaled, 153, 204, 153, "green_hscaled"); addMarkerPolygon3d(vis, yellow3d_hscaled, 204, 204, 153, "yellow_hscaled"); addMarkerPolygon3d(vis, blue3d_hscaled, 153, 153, 204, "blue_hscaled");*/ vis.addPointCloud(cloud); vis.spin(); #endif }
int main (int argc, char *argv[]) { if (argc < 3) { cout << "Enter the two files for registration ..\n"; return -1; } pcl::console::setVerbosityLevel (pcl::console::L_DEBUG); string sourcefile = argv[1]; string targetfile = argv[2]; CloudPtr cloud1 ( new Cloud ); CloudPtr cloud2 ( new Cloud ); readPCDBinaryFile (sourcefile.c_str (), cloud1); readPCDBinaryFile (targetfile.c_str (), cloud2); //pcl::IterativeClosestPointNonLinear <Point, Point> icp; pcl::IterativeClosestPoint <Point, Point> icp; icp.setInputSource (cloud1); icp.setInputTarget (cloud2); icp.setMaximumIterations (2500); icp.setTransformationEpsilon (0.0000001); Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ()); Eigen::Translation3f init_translation (1.79387, 0.720047, 0); //Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity (); Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix (); CloudPtr output (new Cloud); icp.align (*output, init_guess); //pcl::transformPointCloud (*cloud1, *output, icp.getFinalTransformation ()); std::cout << "ICP NL has converged:" << icp.hasConverged () << " score: " << icp.getFitnessScore () << std::endl; cout << "--------- Final transformation ---------------\n"; cout << icp.getFinalTransformation () << "\n\n"; CloudPtr cloud1_ig (new Cloud); pcl::transformPointCloud (*cloud1, *cloud1_ig, init_guess); PCLVisualizer* p = new PCLVisualizer (argc, argv, "Registration"); int vp1 = 1; p->createViewPort (0.0, 0.0, 0.5, 1.0, vp1); int vp2 = 2; p->createViewPort (0.5, 0.0, 1.0, 1.0, vp2); p->setBackgroundColor (113.0/255, 113.0/255, 154.0/255); int color[3] = { 255, 0, 0}; displayPointCloud (p, cloud1_ig, color, (char *) "opcloud1", vp1); color[0] = 0; color[1] = 255; color[2] = 0; displayPointCloud (p, cloud2, color, (char *) "opcloud2", vp1); color[0] = 0; color[1] = 255; color[2] = 0; displayPointCloud (p, output, color, (char *) "opcloud11", vp2); color[0] = 255; color[1] = 0; color[2] = 0; displayPointCloud (p, cloud2, color, (char *) "opcloud12", vp2); p->spin (); return 0; }
void visualize (const ModelLibrary::HashTable* hash_table) { const ModelLibrary::HashTableCell* cells = hash_table->getVoxels (); size_t max_num_entries = 0; int id3[3], num_cells = hash_table->getNumberOfVoxels (); double cell_center[3]; vtkPoints* sphere_centers = vtkPoints::New (VTK_DOUBLE); vtkDoubleArray* scale = vtkDoubleArray::New (); scale->SetNumberOfComponents(1); // Get the positions of the spheres (one sphere per full cell) for (int i = 0 ; i < num_cells ; ++i, ++cells) { // Does the cell have any entries? if (cells->size ()) { // Insert the center of the current voxel in the point set hash_table->compute3dId (i, id3); hash_table->computeVoxelCenter (id3, cell_center); sphere_centers->InsertNextPoint (cell_center); // Save the number of entries scale->InsertNextValue (static_cast<double> (cells->size ())); // Get the max if (cells->size () > max_num_entries) max_num_entries = cells->size (); } } PCLVisualizer vis; vis.setBackgroundColor (0.1, 0.1, 0.1); // Is there something to visualize? if (max_num_entries) { // Compute the factor which maps all the scale values in (0, 1] double factor = 1.0/static_cast<double> (max_num_entries); // Set the true scale for (vtkIdType i = 0 ; i < scale->GetNumberOfTuples () ; ++i) scale->SetValue(i, factor*scale->GetValue (i)); // Input for the glyph object: the centers + scale vtkPolyData *positions = vtkPolyData::New (); positions->SetPoints (sphere_centers); positions->GetPointData ()->SetScalars (scale); // The spheres vtkSphereSource* sphere_src = vtkSphereSource::New (); sphere_src->SetPhiResolution(8); sphere_src->SetThetaResolution(8); sphere_src->SetRadius(0.5*hash_table->getVoxelSpacing ()[0]); // Now that we have the points and the corresponding scalars, build the glyph object vtkGlyph3D *glyph = vtkGlyph3D::New (); glyph->SetScaleModeToScaleByScalar (); glyph->SetColorModeToColorByScalar (); glyph->SetInput (positions); glyph->SetSource (sphere_src->GetOutput ()); glyph->Update (); vtkSmartPointer<vtkPolyData> glyph_output (glyph->GetOutput ()); vis.addModelFromPolyData(glyph_output); // Cleanup glyph->Delete (); positions->Delete (); sphere_src->Delete (); } vis.spin(); // Cleanup sphere_centers->Delete(); scale->Delete(); }