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