void 
pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
{
  double newOriginX = previous_origin_x + offset_x; 
  double newOriginY = previous_origin_y + offset_y; 
  double newOriginZ = previous_origin_z + offset_z;
  double newLimitX = newOriginX + volume_x; 
  double newLimitY = newOriginY + volume_y; 
  double newLimitZ = newOriginZ + volume_z;
	
  // filter points in the space of the new cube
  PointCloudPtr newCube (new pcl::PointCloud<PointT>);
  // condition
  ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ))); 
  
  // build the filter
  pcl::ConditionalRemoval<PointT> condremAND (true);
  condremAND.setCondition (range_condAND);
  condremAND.setInputCloud (world_);
  condremAND.setKeepOrganized (false);
  
  // apply filter
  condremAND.filter (*newCube);
	
  // filter points that belong to the new slice
  ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
  
  if(offset_x >= 0)
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE,  previous_origin_x + volume_x - 1.0 )));
  else
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT,  previous_origin_x )));
	
  if(offset_y >= 0)
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE,  previous_origin_y + volume_y - 1.0 )));
  else
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT,  previous_origin_y )));
	
  if(offset_z >= 0)
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE,  previous_origin_z + volume_z - 1.0 )));
  else
	range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT,  previous_origin_z )));
  
  // build the filter
  pcl::ConditionalRemoval<PointT> condrem (true);
  condrem.setCondition (range_condOR);
  condrem.setInputCloud (newCube);
  condrem.setKeepOrganized (false);
  // apply filter
  condrem.filter (existing_slice);  
 
  if(existing_slice.points.size () != 0)
  {
	//transform the slice in new cube coordinates
	Eigen::Affine3f transformation; 
	transformation.translation ()[0] = newOriginX;
	transformation.translation ()[1] = newOriginY;
	transformation.translation ()[2] = newOriginZ;
		
	transformation.linear ().setIdentity ();

	transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
	
  }
}

void SystemView::render(const Eigen::Affine3f& parentTrans)
{
	if(size() == 0)
		return;

	Eigen::Affine3f trans = getTransform() * parentTrans;
	
	// draw the list elements (titles, backgrounds, logos)
	const float logoSizeX = logoSize().x() + LOGO_PADDING;

	int logoCount = (int)(mSize.x() / logoSizeX) + 2; // how many logos we need to draw
	int center = (int)(mCamOffset);

	if(mEntries.size() == 1)
		logoCount = 1;

	// draw background extras
	Eigen::Affine3f extrasTrans = trans;
	int extrasCenter = (int)mExtrasCamOffset;
	for(int i = extrasCenter - 1; i < extrasCenter + 2; i++)
	{
		int index = i;
		while(index < 0)
			index += mEntries.size();
		while(index >= (int)mEntries.size())
			index -= mEntries.size();

		extrasTrans.translation() = trans.translation() + Eigen::Vector3f((i - mExtrasCamOffset) * mSize.x(), 0, 0);

		Eigen::Vector2i clipRect = Eigen::Vector2i((int)((i - mExtrasCamOffset) * mSize.x()), 0);
		Renderer::pushClipRect(clipRect, mSize.cast<int>());
		mEntries.at(index).data.backgroundExtras->render(extrasTrans);
		Renderer::popClipRect();
	}

	// fade extras if necessary
	if(mExtrasFadeOpacity)
	{
		Renderer::setMatrix(trans);
		Renderer::drawRect(0.0f, 0.0f, mSize.x(), mSize.y(), 0x00000000 | (unsigned char)(mExtrasFadeOpacity * 255));
	}

	// draw logos
	float xOff = (mSize.x() - logoSize().x())/2 - (mCamOffset * logoSizeX);
	float yOff = (mSize.y() - logoSize().y())/2;

	// background behind the logos
	Renderer::setMatrix(trans);
	Renderer::drawRect(0.f, (mSize.y() - BAND_HEIGHT) / 2, mSize.x(), BAND_HEIGHT, 0xFFFFFFD8);

	Eigen::Affine3f logoTrans = trans;
	for(int i = center - logoCount/2; i < center + logoCount/2 + 1; i++)
	{
		int index = i;
		while(index < 0)
			index += mEntries.size();
		while(index >= (int)mEntries.size())
			index -= mEntries.size();

		logoTrans.translation() = trans.translation() + Eigen::Vector3f(i * logoSizeX + xOff, yOff, 0);

		if(index == mCursor) //scale our selection up
		{
			// selected
			const std::shared_ptr<GuiComponent>& comp = mEntries.at(index).data.logoSelected;
			comp->setOpacity(0xFF);
			comp->render(logoTrans);
		}else{
			// not selected
			const std::shared_ptr<GuiComponent>& comp = mEntries.at(index).data.logo;
			comp->setOpacity(0x80);
			comp->render(logoTrans);
		}
	}

	Renderer::setMatrix(trans);
	Renderer::drawRect(mSystemInfo.getPosition().x(), mSystemInfo.getPosition().y() - 1, mSize.x(), mSystemInfo.getSize().y(), 0xDDDDDD00 | (unsigned char)(mSystemInfo.getOpacity() / 255.f * 0xD8));
	mSystemInfo.render(trans);
}

int main(int argc, char** argv)
{
    if( argc < 2 )
    {
        printPrompt( argv[0] );
        return -1;
    }

    initModule_nonfree();

    // Get Input Data
    ifstream file(argv[1]);
    if ( !file.is_open() )
        return false;
    
    string str;
    
        // Image Name
    getline( file, str ); getline( file, str );
    string image_name = str;
        // Cloud Name
    getline( file, str ); getline( file, str );
    string cloud_name = str;
        // width of images to be created.
    getline( file, str ); getline( file, str );
    int w = atoi(str.c_str());
        // height of images to be created
    getline( file, str ); getline( file, str );
    int h = atoi(str.c_str());
        // resolution of voxel grids
    getline( file, str ); getline( file, str );
    float r = atof(str.c_str());
        // f (distance from pinhole)
    getline( file, str ); getline( file, str );
    float f = atof(str.c_str());
        // thetax (initial rotation about X Axis of map)
    getline( file, str ); getline( file, str );
    float thetaX = atof(str.c_str());
        // thetay (initial rotation about Y Axis of map)
    getline( file, str ); getline( file, str );
    float thetaY = atof(str.c_str());
        // number of points to go to
    getline( file, str ); getline( file, str );
    float nop = atoi(str.c_str());
        // Number of divisions
    getline( file, str ); getline( file, str );
    float divs = atoi(str.c_str());
        // Number of images to return
    getline( file, str ); getline( file, str );
    int numtoreturn = atoi(str.c_str());    
        // Should we load or create photos?
    getline( file, str ); getline( file, str );
    string lorc =str.c_str();
        // Directory to look for photos
    getline( file, str ); getline( file, str );
    string dir =str.c_str();
        // Directory to look for kp and descriptors
    getline( file, str ); getline( file, str );
    string kdir =str.c_str();
        // save photos?
    getline( file, str ); getline( file, str );
    string savePhotos =str.c_str();
    
    file.close();
    // Done Getting Input Data

    map<vector<float>, Mat> imagemap;
    map<vector<float>, Mat> surfmap;
    map<vector<float>, Mat> siftmap;
    map<vector<float>, Mat> orbmap;
    map<vector<float>, Mat> fastmap;
    imagemap.clear();

    vector<KeyPoint> SurfKeypoints;
    vector<KeyPoint> SiftKeypoints;
    vector<KeyPoint> OrbKeypoints;
    vector<KeyPoint> FastKeypoints;
    Mat SurfDescriptors;
    Mat SiftDescriptors;
    Mat OrbDescriptors;
    Mat FastDescriptors;

    int minHessian = 300;

    SurfFeatureDetector SurfDetector (minHessian);
    SiftFeatureDetector SiftDetector (minHessian);
    OrbFeatureDetector OrbDetector (minHessian);
    FastFeatureDetector FastDetector (minHessian);


    SurfDescriptorExtractor SurfExtractor;
    SiftDescriptorExtractor SiftExtractor;
    OrbDescriptorExtractor OrbExtractor;

    if ( !fs::exists( dir ) || lorc == "c" )
    { // Load Point Cloud and render images
        PointCloud<PT>::Ptr cloud (new pcl::PointCloud<PT>);
        io::loadPCDFile<PT>(cloud_name, *cloud);

        Eigen::Affine3f tf = Eigen::Affine3f::Identity();
//.........这里部分代码省略.........

void simulate_callback (const pcl::visualization::KeyboardEvent &event,
                        void* viewer_void)
{
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);
  // I choose v for virtual as s for simulate is takwen
  if (event.getKeySym () == "v" && event.keyDown ())
  {
    std::cout << "v was pressed => simulate cloud" << std::endl;

    std::vector<pcl::visualization::Camera> cams;
    viewer->getCameras(cams);
    
    if (cams.size() !=1){
      std::cout << "n cams not 1 exiting\n"; // for now in case ...
     return; 
    }
   // cout << "n cams: " << cams.size() << "\n";
    pcl::visualization::Camera cam = cams[0];
    


	      
	      Eigen::Affine3f pose;
	      pose = viewer->getViewerPose() ;
	      
	      
     std::cout << cam.pos[0] << " " 
               << cam.pos[1] << " " 
               << cam.pos[2] << " p\n";	      
	      
	     Eigen::Matrix3f m;
	     m =pose.rotation();
	     //All axies use right hand rule. x=red axis, y=green axis, z=blue axis z direction is point into the screen. z \ \ \ -----------> x | | | | | | y 
	      
 cout << pose(0,0)  << " " << pose(0,1) << " " << pose(0,2)  << " " << pose(0,3) << " x0\n";
 cout << pose(1,0) << " " << pose(1,1) << " " << pose(1,2) << " " << pose(1,3) << " x1\n";
  cout << pose(2,0) << " " << pose(2,1)  << " " << pose(2,2) << " " << pose(2,3)<< "x2\n";

  double yaw,pitch, roll;
  wRo_to_euler(m,yaw,pitch,roll);
  
 printf("RPY: %f %f %f\n", roll*180/M_PI,pitch*180/M_PI,yaw*180/M_PI);    

// matrix->GetElement(1,0);
  
 cout << m(0,0)  << " " << m(0,1) << " " << m(0,2)  << " "  << " x0\n";
 cout << m(1,0) << " " << m(1,1) << " " << m(1,2) << " "  << " x1\n";
  cout << m(2,0) << " " << m(2,1)  << " " << m(2,2) << " "<< "x2\n\n";
  
  Eigen::Quaternionf rf;
  rf = Eigen::Quaternionf(m);
  
  
   Eigen::Quaterniond r(rf.w(),rf.x(),rf.y(),rf.z());
  
   Eigen::Isometry3d init_pose;
   init_pose.setIdentity();
   init_pose.translation() << cam.pos[0], cam.pos[1], cam.pos[2];  
   //Eigen::Quaterniond m = euler_to_quat(-1.54, 0, 0);
   init_pose.rotate(r);  
//   
  
    std::stringstream ss;
  print_Isometry3d(init_pose,ss);
  std::cout << "init_pose: " << ss.str() << "\n";
  
  
  
  
  viewer->addCoordinateSystem (1.0,pose);
  
  
  
    double tic = getTime();
    std::stringstream ss2;
    ss2.precision(20);
    ss2 << "simulated_pcl_" << tic << ".pcd";
    capture(init_pose,ss2.str());
    cout << (getTime() -tic) << " sec\n";  
  
  
  
  // these three variables determine the position and orientation of
    // the camera.
	      
	      
//     double lookat[3]; - focal location
//     double eye[3]; - my location
//     double up[3]; - updirection
     
	      
	      
//    std::cout << view[0]  << "," << view[1]  << "," << view[2] 
    
//    cameras.back ().view[2] = renderer->GetActiveCamera ()->GetOrientationWXYZ()[2];    
    
//ViewTransform->GetOrientationWXYZ();    
    
  //  Superclass::OnKeyUp ();
    
//.........这里部分代码省略.........

void Terrain::Draw(int type, const Camera& camera, const Light& light){


	//Get new position of the cube and update the model view matrix
    Eigen::Affine3f wMo;//object to world matrix
    Eigen::Affine3f cMw;
    Eigen::Affine3f proj;

    glUseProgram(m_shader);
#ifdef __APPLE__
    glBindVertexArrayAPPLE(m_vertexArrayObject); 
#else
	glBindVertexArray(m_vertexArrayObject);
#endif
    
    glBindBuffer(GL_ARRAY_BUFFER, m_vertexBufferObject);//use as current buffer
    
    GLint world2camera = glGetUniformLocation(m_shader, "cMw"); 
	GLint projection = glGetUniformLocation(m_shader, "proj");
    GLint kAmbient = glGetUniformLocation(m_shader,"kAmbient");
    GLint kDiffuse = glGetUniformLocation(m_shader,"kDiffuse");
    GLint kSpecular = glGetUniformLocation(m_shader,"kSpecular");
    GLint shininess = glGetUniformLocation(m_shader,"shininess");
    GLint camera_position = glGetUniformLocation(m_shader, "cameraPosition");
    GLint light_position = glGetUniformLocation(m_shader, "lightPosition");
    
    //generate the Angel::Angel::Angel::matrixes
    proj = Util::Perspective( camera.m_fovy, camera.m_aspect, camera.m_znear, camera.m_zfar );
	cMw = camera.m_cMw;//LookAt(camera.position,camera.lookat, camera.up );
    
    Eigen::Vector4f v4color(0.55,0.25,0.08,1.0);
    Eigen::Vector4f Ambient;
    Ambient = 0.3*v4color;
    Eigen::Vector4f Diffuse;
    Diffuse = 0.5*v4color;
    Eigen::Vector4f Specular(0.3,0.3,0.3,1.0);
    
    glUniformMatrix4fv( world2camera, 1, GL_FALSE, cMw.data() );
    glUniformMatrix4fv( projection, 1, GL_FALSE, proj.data() );
    
    glUniform4fv(kAmbient, 1, Ambient.data());
    glUniform4fv(kDiffuse, 1, Diffuse.data()); 
    glUniform4fv(kSpecular, 1, Specular.data());
    glUniform4fv(camera_position, 1, camera.m_position.data());
    glUniform4fv(light_position, 1, light.m_position.data());
    glUniform1f(shininess, 10);

    switch (type) {
        case DRAW_MESH:
            glUniform1i(glGetUniformLocation(m_shader, "renderType"), 1);
            glDrawArrays(GL_LINES, 0, m_NTrianglePoints);
            break;
        case DRAW_PHONG:
            glUniform1i(glGetUniformLocation(m_shader, "renderType"), 2);
            glDrawArrays(GL_TRIANGLES, 0, m_NTrianglePoints);
            break;
    }

	//draw the obstacles
	for(int i = 0; i < m_obstacles.size(); i++)
	{
		m_obstacles[i]->Draw(type,camera, light);
	}

	for(int i = 0; i < m_foods.size(); i++)
	{
		m_foods[i]->Draw(type,camera, light);
	}

	//draw the obstacles
	for(int i = 0; i < m_surface_objects.size(); i++)
	{
		m_surface_objects[i]->Draw(type,camera, light);
	}

}

void keypoint_map::optimize() {

	g2o::SparseOptimizer optimizer;
	optimizer.setVerbose(true);
	g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

	linearSolver = new g2o::LinearSolverCholmod<
			g2o::BlockSolver_6_3::PoseMatrixType>();

	g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* solver =
			new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	optimizer.setAlgorithm(solver);

	double focal_length = intrinsics[0];
	Eigen::Vector2d principal_point(intrinsics[2], intrinsics[3]);

	g2o::CameraParameters * cam_params = new g2o::CameraParameters(focal_length,
			principal_point, 0.);
	cam_params->setId(0);

	if (!optimizer.addParameter(cam_params)) {
		assert(false);
	}

	std::cerr << camera_positions.size() << " " << keypoints3d.size() << " "
			<< observations.size() << std::endl;

	int vertex_id = 0, point_id = 0;

	for (size_t i = 0; i < camera_positions.size(); i++) {
		Eigen::Affine3f cam_world = camera_positions[i].inverse();
		Eigen::Vector3d trans(cam_world.translation().cast<double>());
		Eigen::Quaterniond q(cam_world.rotation().cast<double>());

		g2o::SE3Quat pose(q, trans);
		g2o::VertexSE3Expmap * v_se3 = new g2o::VertexSE3Expmap();
		v_se3->setId(vertex_id);
		if (i < 1) {
			v_se3->setFixed(true);
		}
		v_se3->setEstimate(pose);
		optimizer.addVertex(v_se3);
		vertex_id++;
	}

	for (size_t i = 0; i < keypoints3d.size(); i++) {
		g2o::VertexSBAPointXYZ * v_p = new g2o::VertexSBAPointXYZ();
		v_p->setId(vertex_id + point_id);
		v_p->setMarginalized(true);
		v_p->setEstimate(keypoints3d[i].getVector3fMap().cast<double>());
		optimizer.addVertex(v_p);
		point_id++;
	}

	for (size_t i = 0; i < observations.size(); i++) {
		g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
		e->setVertex(0,
				dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(
						vertex_id + observations[i].point_id)->second));
		e->setVertex(1,
				dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(
						observations[i].cam_id)->second));
		e->setMeasurement(observations[i].coord.cast<double>());
		e->information() = Eigen::Matrix2d::Identity();

		g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
		e->setRobustKernel(rk);

		e->setParameterId(0, 0);
		optimizer.addEdge(e);

	}

	//optimizer.save("debug.txt");

	optimizer.initializeOptimization();
	optimizer.setVerbose(true);

	std::cout << std::endl;
	std::cout << "Performing full BA:" << std::endl;
	optimizer.optimize(1);
	std::cout << std::endl;

	for (int i = 0; i < vertex_id; i++) {
		g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(
				i);
		if (v_it == optimizer.vertices().end()) {
			std::cerr << "Vertex " << i << " not in graph!" << std::endl;
			exit(-1);
		}

		g2o::VertexSE3Expmap * v_c =
				dynamic_cast<g2o::VertexSE3Expmap *>(v_it->second);
		if (v_c == 0) {
			std::cerr << "Vertex " << i << "is not a VertexSE3Expmap!"
					<< std::endl;
			exit(-1);
		}

//.........这里部分代码省略.........

void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
                                                    const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
  JSK_ROS_DEBUG("DepthImageCreator::publish_points");
  if (!pcloud2)  return;
  bool proc_cloud = true, proc_image = true, proc_disp = true;
  if ( pub_cloud_.getNumSubscribers()==0 ) {
    proc_cloud = false;
  }
  if ( pub_image_.getNumSubscribers()==0 ) {
    proc_image = false;
  }
  if ( pub_disp_image_.getNumSubscribers()==0 ) {
    proc_disp = false;
  }
  if( !proc_cloud && !proc_image && !proc_disp) return;

  int width = info->width;
  int height = info->height;
  float fx = info->P[0];
  float cx = info->P[2];
  float tx = info->P[3];
  float fy = info->P[5];
  float cy = info->P[6];

  Eigen::Affine3f sensorPose;
  {
    tf::StampedTransform transform;
    if(use_fixed_transform) {
      transform = fixed_transform;
    } else {
      try {
        tf_listener_->waitForTransform(pcloud2->header.frame_id,
                                       info->header.frame_id,
                                       info->header.stamp,
                                       ros::Duration(0.001));
        tf_listener_->lookupTransform(pcloud2->header.frame_id,
                                      info->header.frame_id,
                                      info->header.stamp, transform);
      }
      catch ( std::runtime_error e ) {
        JSK_ROS_ERROR("%s",e.what());
        return;
      }
    }
    tf::Vector3 p = transform.getOrigin();
    tf::Quaternion q = transform.getRotation();
    sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
    Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
    sensorPose = sensorPose * rot;

    if (tx != 0.0) {
      Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
      sensorPose = sensorPose * trans;
    }
#if 0 // debug print
    JSK_ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
             sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
             sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
             sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
             sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
#endif
  }

  PointCloud pointCloud;
  pcl::RangeImagePlanar rangeImageP;
  {
    // code here is dirty, some bag is in RangeImagePlanar
    PointCloud tpc;
    pcl::fromROSMsg(*pcloud2, tpc);

    Eigen::Affine3f inv;
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
    inv = sensorPose.inverse();
    pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
#else
    pcl::getInverse(sensorPose, inv);
    pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
#endif

    Eigen::Affine3f dummytrans;
    dummytrans.setIdentity();
    rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
                                                   width/scale_depth, height/scale_depth,
                                                   cx/scale_depth, cy/scale_depth,
                                                   fx/scale_depth, fy/scale_depth,
                                                   dummytrans); //sensorPose);
  }

  cv::Mat mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
  float *tmpf = (float *)mat.ptr();
  for(unsigned int i = 0; i < rangeImageP.height*rangeImageP.width; i++) {
    tmpf[i] = rangeImageP.points[i].z;
  }

  if(scale_depth != 1.0) {
    cv::Mat tmpmat(info->height, info->width, CV_32FC1);
    cv::resize(mat, tmpmat, cv::Size(info->width, info->height)); // LINEAR
    //cv::resize(mat, tmpmat, cv::Size(info->width, info->height), 0.0, 0.0, cv::INTER_NEAREST);
    mat = tmpmat;
  }
//.........这里部分代码省略.........

	void cloud_cb(
			const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud) {

		iter++;
		if(iter != skip) return;
		iter = 0;

		pcl::PointCloud<pcl::PointXYZRGB> cloud_transformed,
				cloud_aligned, cloud_filtered;

		Eigen::Affine3f view_transform;
		view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;

		Eigen::AngleAxis<float> rot(tilt * M_PI / 180,
				Eigen::Vector3f(0, 1, 0));

		view_transform.prerotate(rot);

		pcl::transformPointCloud(*cloud, cloud_transformed, view_transform);

		pcl::ModelCoefficients::Ptr coefficients(
				new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

		pcl::SACSegmentation<pcl::PointXYZRGB> seg;

		seg.setOptimizeCoefficients(true);

		seg.setModelType(pcl::SACMODEL_PLANE);
		seg.setMethodType(pcl::SAC_RANSAC);
		seg.setDistanceThreshold(0.05);
		seg.setProbability(0.99);

		seg.setInputCloud(cloud_transformed.makeShared());
		seg.segment(*inliers, *coefficients);

		pcl::ExtractIndices<pcl::PointXYZRGB> extract;

		extract.setInputCloud(cloud_transformed.makeShared());
		extract.setIndices(inliers);
		extract.setNegative(true);
		extract.filter(cloud_transformed);

		std::cout << "Z vector: " << coefficients->values[0] << " "
				<< coefficients->values[1] << " " << coefficients->values[2]
				<< " " << coefficients->values[3] << std::endl;

		Eigen::Vector3f z_current(coefficients->values[0],
				coefficients->values[1], coefficients->values[2]);
		Eigen::Vector3f y(0, 1, 0);

		Eigen::Affine3f rotation;
		rotation = pcl::getTransFromUnitVectorsZY(z_current, y);
		rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3]));

		pcl::transformPointCloud(cloud_transformed, cloud_aligned, rotation);

		Eigen::Affine3f res = (rotation * view_transform);

		cloud_aligned.sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1);
		cloud_aligned.sensor_orientation_ = res.rotate(
				Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();

		seg.setInputCloud(cloud_aligned.makeShared());
		seg.segment(*inliers, *coefficients);

		std::cout << "Z vector: " << coefficients->values[0] << " "
				<< coefficients->values[1] << " " << coefficients->values[2]
				<< " " << coefficients->values[3] << std::endl;

		pub.publish(cloud_aligned);



	}

void IterativeClosestPoint::execute() {
    float error = std::numeric_limits<float>::max(), previousError;
    uint iterations = 0;

    // Get access to the two point sets
    PointSetAccess::pointer accessFixedSet = ((PointSet::pointer)getStaticInputData<PointSet>(0))->getAccess(ACCESS_READ);
    PointSetAccess::pointer accessMovingSet = ((PointSet::pointer)getStaticInputData<PointSet>(1))->getAccess(ACCESS_READ);

    // Get transformations of point sets
    AffineTransformation::pointer fixedPointTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(0));
    Eigen::Affine3f fixedPointTransform;
    fixedPointTransform.matrix() = fixedPointTransform2->matrix();
    AffineTransformation::pointer initialMovingTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(1));
    Eigen::Affine3f initialMovingTransform;
    initialMovingTransform.matrix() = initialMovingTransform2->matrix();

    // These matrices are Nx3
    MatrixXf fixedPoints = accessFixedSet->getPointSetAsMatrix();
    MatrixXf movingPoints = accessMovingSet->getPointSetAsMatrix();

    Eigen::Affine3f currentTransformation = Eigen::Affine3f::Identity();

    // Want to choose the smallest one as moving
    bool invertTransform = false;
    if(false && fixedPoints.cols() < movingPoints.cols()) {
        reportInfo() << "switching fixed and moving" << Reporter::end;
        // Switch fixed and moving
        MatrixXf temp = fixedPoints;
        fixedPoints = movingPoints;
        movingPoints = temp;
        invertTransform = true;

        // Apply initial transformations
        //currentTransformation = fixedPointTransform.getTransform();
        movingPoints = fixedPointTransform*movingPoints.colwise().homogeneous();
        fixedPoints = initialMovingTransform*fixedPoints.colwise().homogeneous();
    } else {
        // Apply initial transformations
        //currentTransformation = initialMovingTransform.getTransform();
        movingPoints = initialMovingTransform*movingPoints.colwise().homogeneous();
        fixedPoints = fixedPointTransform*fixedPoints.colwise().homogeneous();
    }
    do {
        previousError = error;
        MatrixXf movedPoints = currentTransformation*(movingPoints.colwise().homogeneous());

        // Match closest points using current transformation
        MatrixXf rearrangedFixedPoints = rearrangeMatrixToClosestPoints(
                fixedPoints, movedPoints);

        // Get centroids
        Vector3f centroidFixed = getCentroid(rearrangedFixedPoints);
        //reportInfo() << "Centroid fixed: " << Reporter::end;
        //reportInfo() << centroidFixed << Reporter::end;
        Vector3f centroidMoving = getCentroid(movedPoints);
        //reportInfo() << "Centroid moving: " << Reporter::end;
        //reportInfo() << centroidMoving << Reporter::end;

        Eigen::Transform<float, 3, Eigen::Affine> updateTransform = Eigen::Transform<float, 3, Eigen::Affine>::Identity();

        if(mTransformationType == IterativeClosestPoint::RIGID) {
            // Create correlation matrix H of the deviations from centroid
            MatrixXf H = (movedPoints.colwise() - centroidMoving)*
                    (rearrangedFixedPoints.colwise() - centroidFixed).transpose();

            // Do SVD on H
            Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);

            // Estimate rotation as R=V*U.transpose()
            MatrixXf temp = svd.matrixV()*svd.matrixU().transpose();
            Matrix3f d = Matrix3f::Identity();
            d(2,2) = sign(temp.determinant());
            Matrix3f R = svd.matrixV()*d*svd.matrixU().transpose();

            // Estimate translation
            Vector3f T = centroidFixed - R*centroidMoving;

            updateTransform.linear() = R;
            updateTransform.translation() = T;
        } else {
            // Only translation
            Vector3f T = centroidFixed - centroidMoving;
            updateTransform.translation() = T;
        }

        // Update current transformation
        currentTransformation = updateTransform*currentTransformation;

        // Calculate RMS error
        MatrixXf distance = rearrangedFixedPoints - currentTransformation*(movingPoints.colwise().homogeneous());
        error = 0;
        for(uint i = 0; i < distance.cols(); i++) {
            error += pow(distance.col(i).norm(),2);
        }
        error = sqrt(error / distance.cols());

        iterations++;
        reportInfo() << "Error: " << error << Reporter::end;
        // To continue, change in error has to be above min error change and nr of iterations less than max iterations
    } while(previousError-error > mMinErrorChange && iterations < mMaxIterations);
//.........这里部分代码省略.........

void simulate_callback (const pcl::visualization::KeyboardEvent &event,
                        void* viewer_void)
{
  pcl::visualization::PCLVisualizer::Ptr viewer = *static_cast<pcl::visualization::PCLVisualizer::Ptr *> (viewer_void);
  // I choose v for virtual as s for simulate is takwen
  if (event.getKeySym () == "v" && event.keyDown ())
  {
    std::cout << "v was pressed => simulate cloud" << std::endl;

    std::vector<pcl::visualization::Camera> cams;
    viewer->getCameras(cams);
    
    if (cams.size() !=1){
      std::cout << "n cams not 1 exiting\n"; // for now in case ...
     return; 
    }
   // cout << "n cams: " << cams.size() << "\n";
    pcl::visualization::Camera cam = cams[0];
    


	      
	      Eigen::Affine3f pose;
	      pose = viewer->getViewerPose() ;
	      
	      
     std::cout << cam.pos[0] << " " 
               << cam.pos[1] << " " 
               << cam.pos[2] << " p\n";	      
	      
	     Eigen::Matrix3f m;
	     m =pose.rotation();
	     //All axies use right hand rule. x=red axis, y=green axis, z=blue axis z direction is point into the screen. z \ \ \ -----------> x | | | | | | y 
	      
 cout << pose(0,0)  << " " << pose(0,1) << " " << pose(0,2)  << " " << pose(0,3) << " x0\n";
 cout << pose(1,0) << " " << pose(1,1) << " " << pose(1,2) << " " << pose(1,3) << " x1\n";
  cout << pose(2,0) << " " << pose(2,1)  << " " << pose(2,2) << " " << pose(2,3)<< "x2\n";

  double yaw,pitch, roll;
  wRo_to_euler(m,yaw,pitch,roll);
  
 printf("RPY: %f %f %f\n", roll*180/M_PI,pitch*180/M_PI,yaw*180/M_PI);    

// matrix->GetElement(1,0);
  
 cout << m(0,0)  << " " << m(0,1) << " " << m(0,2)  << " "  << " x0\n";
 cout << m(1,0) << " " << m(1,1) << " " << m(1,2) << " "  << " x1\n";
  cout << m(2,0) << " " << m(2,1)  << " " << m(2,2) << " "<< "x2\n\n";
  
  Eigen::Quaternionf rf;
  rf = Eigen::Quaternionf(m);
  
  
   Eigen::Quaterniond r(rf.w(),rf.x(),rf.y(),rf.z());
  
   Eigen::Isometry3d init_pose;
   init_pose.setIdentity();
   init_pose.translation() << cam.pos[0], cam.pos[1], cam.pos[2];  
   //Eigen::Quaterniond m = euler_to_quat(-1.54, 0, 0);
   init_pose.rotate(r);  
//   
  
    std::stringstream ss;
  print_Isometry3d(init_pose,ss);
  std::cout << "init_pose: " << ss.str() << "\n";
  
  
  
  
  viewer->addCoordinateSystem (1.0,pose,"reference");
  
  
  
    double tic = getTime();
    std::stringstream ss2;
    ss2.precision(20);
    ss2 << "simulated_pcl_" << tic << ".pcd";
    capture(init_pose);
    cout << (getTime() -tic) << " sec\n";  
  }
}

void ShapeVisualization::setShapePosition(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)//,const cob_3d_mapping_msgs::Shape& shape)
{

  cob_3d_mapping_msgs::ShapeArray map_msg;
  map_msg.header.frame_id = frame_id_;
  map_msg.header.stamp = ros::Time::now();

  int shape_id,index;
  index=-1;
  stringstream name(feedback->marker_name);

  Eigen::Quaternionf quat;

  Eigen::Matrix3f rotationMat;
  Eigen::MatrixXf rotationMatInit;

  Eigen::Vector3f normalVec;
  Eigen::Vector3f normalVecNew;
  Eigen::Vector3f newCentroid;
  Eigen::Matrix4f transSecondStep;


  if (feedback->marker_name != "Text"){
    name >> shape_id ;
    cob_3d_mapping::Polygon p;

    for(unsigned int i=0;i<sha.shapes.size();++i)
    {
    	if (sha.shapes[i].id == shape_id)
	{
		index = i;
	}
    }
    // temporary fix.
    //do nothing if index of shape is not found
    // this is not supposed to occur , but apparently it does
    if(index==-1){
    ROS_WARN("shape not in map array");
    return;
	}

    cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

    if (feedback->event_type == 2 && feedback->menu_entry_id == 5){
      quatInit.x() = (float)feedback->pose.orientation.x ;           //normalized
      quatInit.y() = (float)feedback->pose.orientation.y ;
      quatInit.z() = (float)feedback->pose.orientation.z ;
      quatInit.w() = (float)feedback->pose.orientation.w ;

      oldCentroid (0) = (float)feedback->pose.position.x ;
      oldCentroid (1) = (float)feedback->pose.position.y ;
      oldCentroid (2) = (float)feedback->pose.position.z ;

      quatInit.normalize() ;

      rotationMatInit = quatInit.toRotationMatrix() ;

      transInit.block(0,0,3,3) << rotationMatInit ;
      transInit.col(3).head(3) << oldCentroid(0) , oldCentroid(1), oldCentroid(2) ;
      transInit.row(3) << 0,0,0,1 ;

      transInitInv = transInit.inverse() ;
      Eigen::Affine3f affineInitFinal (transInitInv) ;
      affineInit = affineInitFinal ;

      std::cout << "transInit : " << "\n"    << affineInitFinal.matrix() << "\n" ;
    }

    if (feedback->event_type == 5){
      /* the name of the marker is arrows_shape_.id, we need to erase the "arrows_" part */
      //string strName(feedback->marker_name);
      //strName.erase(strName.begin(),strName.begin()+7);
//      stringstream name(strName);
	stringstream name(feedback->marker_name);

      /* the name of the marker is arrows_shape_.id, we need to erase the "arrows_" part */
      //      int test ;
      //      string strName(feedback->marker_name);
      //      strName.erase(strName.begin(),strName.begin()+7);
      //      stringstream nameTest(strName);
      //      std::cout << "nameTest : " << nameTest.str() << "\n" ;
      //      nameTest >> test ;
      //      std::cout << "test : " << test << "\n" ;

      //      name >> shape_id ;
      cob_3d_mapping::Polygon p;
      //      for(size_t i=0;i<sha.shapes.size();++i)
      //      {
      //        if (sha.shapes[i].id == shape_id)
      //        {
      //          index = i;
      //        }
      //      }
      std::cout << "index is : " << index << "\n" ;
      cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

      quat.x() = (float)feedback->pose.orientation.x ;           //normalized
      quat.y() = (float)feedback->pose.orientation.y ;
      quat.z() = (float)feedback->pose.orientation.z ;
      quat.w() = (float)feedback->pose.orientation.w ;
//.........这里部分代码省略.........

  void PointCloudLocalization::cloudCallback(
    const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
  {
    vital_checker_->poke();
    boost::mutex::scoped_lock lock(mutex_);
    //JSK_NODELET_INFO("cloudCallback");
    latest_cloud_ = cloud_msg;
    if (localize_requested_){
      JSK_NODELET_INFO("localization is requested");
      try {
        pcl::PointCloud<pcl::PointNormal>::Ptr
          local_cloud (new pcl::PointCloud<pcl::PointNormal>);
        pcl::fromROSMsg(*latest_cloud_, *local_cloud);
        JSK_NODELET_INFO("waiting for tf transformation from %s tp %s",
                     latest_cloud_->header.frame_id.c_str(),
                     global_frame_.c_str());
        if (tf_listener_->waitForTransform(
              latest_cloud_->header.frame_id,
              global_frame_,
              latest_cloud_->header.stamp,
              ros::Duration(1.0))) {
          pcl::PointCloud<pcl::PointNormal>::Ptr
            input_cloud (new pcl::PointCloud<pcl::PointNormal>);
          if (use_normal_) {
            pcl_ros::transformPointCloudWithNormals(global_frame_,
                                                    *local_cloud,
                                                    *input_cloud,
                                                    *tf_listener_);
          }
          else {
            pcl_ros::transformPointCloud(global_frame_,
                                         *local_cloud,
                                         *input_cloud,
                                         *tf_listener_);
          }
          pcl::PointCloud<pcl::PointNormal>::Ptr
            input_downsampled_cloud (new pcl::PointCloud<pcl::PointNormal>);
          applyDownsampling(input_cloud, *input_downsampled_cloud);
          if (isFirstTime()) {
            all_cloud_ = input_downsampled_cloud;
            first_time_ = false;
          }
          else {
            // run ICP
            ros::ServiceClient client
              = pnh_->serviceClient<jsk_pcl_ros::ICPAlign>("icp_align");
            jsk_pcl_ros::ICPAlign icp_srv;

            if (clip_unseen_pointcloud_) {
              // Before running ICP, remove pointcloud where we cannot see
              // First, transform reference pointcloud, that is all_cloud_, into
              // sensor frame.
              // And after that, remove points which are x < 0.
              tf::StampedTransform global_sensor_tf_transform
                = lookupTransformWithDuration(
                  tf_listener_,
                  global_frame_,
                  sensor_frame_,
                  cloud_msg->header.stamp,
                  ros::Duration(1.0));
              Eigen::Affine3f global_sensor_transform;
              tf::transformTFToEigen(global_sensor_tf_transform,
                                     global_sensor_transform);
              pcl::PointCloud<pcl::PointNormal>::Ptr sensor_cloud
                (new pcl::PointCloud<pcl::PointNormal>);
              pcl::transformPointCloudWithNormals(
                *all_cloud_,
                *sensor_cloud,
                global_sensor_transform.inverse());
              // Remove negative-x points
              pcl::PassThrough<pcl::PointNormal> pass;
              pass.setInputCloud(sensor_cloud);
              pass.setFilterFieldName("x");
              pass.setFilterLimits(0.0, 100.0);
              pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud
                (new pcl::PointCloud<pcl::PointNormal>);
              pass.filter(*filtered_cloud);
              JSK_NODELET_INFO("clipping: %lu -> %lu", sensor_cloud->points.size(), filtered_cloud->points.size());
              // Convert the pointcloud to global frame again
              pcl::PointCloud<pcl::PointNormal>::Ptr global_filtered_cloud
                (new pcl::PointCloud<pcl::PointNormal>);
              pcl::transformPointCloudWithNormals(
                *filtered_cloud,
                *global_filtered_cloud,
                global_sensor_transform);
              pcl::toROSMsg(*global_filtered_cloud,
                            icp_srv.request.target_cloud);
            }
            else {
              pcl::toROSMsg(*all_cloud_,
                            icp_srv.request.target_cloud);
            }
            pcl::toROSMsg(*input_downsampled_cloud,
                          icp_srv.request.reference_cloud);
            
            if (client.call(icp_srv)) {
              Eigen::Affine3f transform;
              tf::poseMsgToEigen(icp_srv.response.result.pose, transform);
              Eigen::Vector3f transform_pos(transform.translation());
              float roll, pitch, yaw;
//.........这里部分代码省略.........

void
pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
  // Resize output cloud to sample size
  output.data.resize (input_->data.size ());
  removed_indices_->resize (input_->data.size ());

  // Copy the common fields
  output.fields = input_->fields;
  output.is_bigendian = input_->is_bigendian;
  output.row_step = input_->row_step;
  output.point_step = input_->point_step;
  output.height = 1;

  int indices_count = 0;
  int removed_indices_count = 0;

  Eigen::Affine3f transform = Eigen::Affine3f::Identity();
  Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity();

  if (rotation_ != Eigen::Vector3f::Zero ())
  {
    pcl::getTransformation (0, 0, 0,
                            rotation_ (0), rotation_ (1), rotation_ (2),
                            transform);
    inverse_transform = transform.inverse();
  }

  //PointXYZ local_pt;
  Eigen::Vector3f local_pt (Eigen::Vector3f::Zero ());

  for (size_t index = 0; index < indices_->size (); ++index)
  {
    // Get local point
    int point_offset = ((*indices_)[index] * input_->point_step);
    int offset = point_offset + input_->fields[x_idx_].offset;
    memcpy (&local_pt, &input_->data[offset], sizeof (float)*3);

    // Check if the point is invalid
    if (!pcl_isfinite (local_pt.x ()) ||
        !pcl_isfinite (local_pt.y ()) ||
        !pcl_isfinite (local_pt.z ()))
      continue;

    // Transform point to world space
    if (!(transform_.matrix().isIdentity()))
      local_pt = transform_ * local_pt;

    if (translation_ != Eigen::Vector3f::Zero ())
    {
      local_pt.x () = local_pt.x () - translation_ (0);
      local_pt.y () = local_pt.y () - translation_ (1);
      local_pt.z () = local_pt.z () - translation_ (2);
    }

    // Transform point to local space of crop box
    if (!(inverse_transform.matrix ().isIdentity ()))
      local_pt = inverse_transform * local_pt;

    // If outside the cropbox
    if ( (local_pt.x () < min_pt_[0] || local_pt.y () < min_pt_[1] || local_pt.z () < min_pt_[2]) ||
         (local_pt.x () > max_pt_[0] || local_pt.y () > max_pt_[1] || local_pt.z () > max_pt_[2]))
    {
      if (negative_)
      {
        memcpy (&output.data[indices_count++ * output.point_step],
                &input_->data[index * output.point_step], output.point_step);
      }
      else if (extract_removed_indices_)
      {
        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
      }
    }