/* ******************************************************************************************** */
int main () {
	
	for(int i = 0; i < 4; i++) cs.push_back(Cloud());
	pthread_mutex_init(&lock, NULL);
	SimpleOpenNIViewer v1(1), v2(2);
	v1.init();
	v2.init();

	// Create the viewer
	Viewer* viewer = new Viewer ("3D Viewer");
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample");

	// Visualize
	while(true) {
		Cloud c;
    pthread_mutex_lock(&lock);
		for(int i = 0; i < 2; i++) {
			printf("%d\t", cs[i].size());
			c += (cs[i]);
		}
		printf("\n");
    pthread_mutex_unlock(&lock);

		viewer->removePointCloud();
		Cloud::Ptr cp = c.makeShared();
		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(cp);
		viewer->addPointCloud(cp, rgb);
		viewer->spinOnce();
	//	printf("%d\n", c.size());
	}
}
Example #2
0
/* ******************************************************************************************** */
int main () {
	
	// Compute the transformation
	T1 = T2 = T3 = T4 = Eigen::Matrix4d::Identity();
	T1.block<3,3>(0,0) = (Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,0,1)) *
		Eigen::AngleAxis<double>(M_PI_2, Eigen::Vector3d(0,1,0))).matrix();
	T1.block<3,1>(0,3) = Eigen::Vector3d(2.74, -0.45, 2.2);
	T2.block<3,3>(0,0) = (Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,0,1)) *
		Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,1,0))).matrix();
	T2.block<3,1>(0,3) = Eigen::Vector3d(0.15, -0.11, 4.39);
	T3.block<3,3>(0,0) = (Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,0,1)) *
		Eigen::AngleAxis<double>(3*M_PI_2, Eigen::Vector3d(0,1,0))).matrix();
	T3.block<3,1>(0,3) = Eigen::Vector3d(-1.41, -0.06, 3.05);
	T4.block<3,3>(0,0) = (Eigen::AngleAxis<double>(M_PI, Eigen::Vector3d(0,0,1)) *
		Eigen::AngleAxis<double>(0, Eigen::Vector3d(0,1,0))).matrix();
	T = &T1;

	for(int i = 0; i < 4; i++) cs.push_back(Cloud::Ptr(new Cloud));
	pthread_mutex_init(&lock, NULL);
	pthread_mutex_init(&lock2, NULL);
	SimpleOpenNIViewer v1(1,0), v2(3,1), v3(2,2), v4(4,3);
	v1.init();
	v2.init();
	v3.init();
	v4.init();

	// Create the viewer
	char buf [256];
	Viewer* viewer = new Viewer ("All 4");
  viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample");

	// Visualize
	int counter = 0;
	while(true) {
    pthread_mutex_lock(&lock2);
    pthread_mutex_unlock(&lock2);
    pthread_mutex_lock(&lock);
		Cloud::Ptr c (new Cloud);
		Cloud::Ptr c1, c2,c3,c4;
		c1 = cs[0];
		c2 = cs[1];
		c3 = cs[2];
		c4 = cs[3];
		counter++;

		// Save the data
		if(counter % 10 == 0) {
			char buf1 [256];
			char buf2 [256];
			char buf3 [256];
			char buf4 [256];
			sprintf(buf1, "kin1-count%04d", counter/4);
			sprintf(buf2, "kin2-count%04d", counter/4);
			sprintf(buf3, "kin3-count%04d", counter/4);
			sprintf(buf4, "kin4-count%04d", counter/4);
			pcl::io::savePCDFileASCII (buf1, *c1);
			pcl::io::savePCDFileASCII (buf2, *c2);
			pcl::io::savePCDFileASCII (buf3, *c3);
			pcl::io::savePCDFileASCII (buf4, *c4);
		}
		
		double distLimit = 2.5;

		printf("seeAll: %d\n", seeAll);
		if(seeAll || T == &T1) {
			if(counter % 4 == 0) cout << "====== " << counter << " ==============================================\nT1: \n" << T1 << endl;
			for (int h=0; h < c1->height; h++) {
				for (int w=0; w < c1->width; w++) {
					pcl::PointXYZRGBA point = c1->at(w, h);
					if(point.x != point.x) continue;
					if(point.z > distLimit) continue;
					Eigen::Vector4d p (point.x, point.y, point.z, 1);
					Eigen::Vector4d Tp = T1 * p;
					point.x = Tp(0); point.y = Tp(1); point.z = Tp(2);
					c->push_back(point);
				}
			}
		}

		if(seeAll || T == &T2) {
			if(counter % 4 == 0) cout << "====== " << counter << " ==============================================\nT2: \n" << T2 << endl;
			for (int h=0; h < c2->height; h++) {
				for (int w=0; w < c2->width; w++) {
					pcl::PointXYZRGBA point = c2->at(w, h);
					if(point.x != point.x) continue;
					if(point.z > distLimit) continue;
					Eigen::Vector4d p (point.x, point.y, point.z, 1);
					Eigen::Vector4d Tp = T2 * p;
					point.x = Tp(0); point.y = Tp(1); point.z = Tp(2);
					c->push_back(point);
				}
			}
		}

		if(seeAll || T == &T3) {
			if(counter % 4 == 0) cout << "====== " << counter << " ==============================================\nT3: \n" << T3 << endl;
			for (int h=0; h < c3->height; h++) {
				for (int w=0; w < c3->width; w++) {
					pcl::PointXYZRGBA point = c3->at(w, h);
					if(point.x != point.x) continue;
					if(point.z > distLimit) continue;
					Eigen::Vector4d p (point.x, point.y, point.z, 1);
					Eigen::Vector4d Tp = T3 * p;
					point.x = Tp(0); point.y = Tp(1); point.z = Tp(2);
					c->push_back(point);
				}
			}
		}

		for (int h=0; h < c4->height; h++) {
			for (int w=0; w < c4->width; w++) {
				pcl::PointXYZRGBA point = c4->at(w, h);
				if(point.x != point.x) continue;
				if(point.z > distLimit) continue;
				Eigen::Vector4d p (point.x, point.y, point.z, 1);
				Eigen::Vector4d Tp = T4 * p;
				point.x = Tp(0); point.y = Tp(1); point.z = Tp(2);
				c->push_back(point);
			}
		}



//		c += c2;
    pthread_mutex_unlock(&lock);

		viewer->removePointCloud();
		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(c);
		viewer->addPointCloud(c, rgb);
		viewer->spinOnce();
	//	printf("%d\n", c.size());
	}
}