int
vpm(int argc, char **argv)
{
	if( argc < 2 )
		throw std::runtime_error("Provide model name as second argument");
	std::string obj_name(argv[1]);
	std::string vision_module_dir( ros::package::getPath("vision_module") );
	std::string tree_vps_path( vision_module_dir + "/data/tree_vps.txt");
	std::string views_path( vision_module_dir + "/../database/cloud_data/"
														   + obj_name + "_views" );
														   
														   
	// Read the tree vps file
	Eigen::MatrixXd tree_vps;
	io_utils::file2matrix( tree_vps_path, tree_vps, 3 );
	int num_vps = tree_vps.rows();
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_model_ptr( new pcl::PointCloud<pcl::PointXYZ>);
	for(int vp = 0; vp < num_vps; ++vp)
	{
		// load the view
		pcl::PointCloud<pcl::PointXYZ>::Ptr view_ptr( new pcl::PointCloud<pcl::PointXYZ>);
		std::string view_path( views_path + "/" + obj_name + "_" 
													 + boost::lexical_cast<std::string>(vp) +".pcd" );
		pcl::io::loadPCDFile<pcl::PointXYZ> ( view_path, *view_ptr );
		
		// rotate to world frame
		Eigen::Vector3d camera_position = tree_vps.row(vp).transpose();
		Eigen::Vector4d camera_orientation = misc::target2quat( camera_position, 
																				  Eigen::Vector3d(0,0,0) );
		
		tf::Transform map_T_snsr;
		map_T_snsr.setOrigin( tf::Vector3(camera_position.x(), 
													 camera_position.y(), 
													 camera_position.z()) );
		map_T_snsr.setRotation( tf::Quaternion( camera_orientation.x(), 
															 camera_orientation.y(), 
															 camera_orientation.z(), 
															 camera_orientation.w()) );
		
		tf::Transform snsr_T_optical;
		snsr_T_optical.setOrigin( tf::Vector3( 0.0, 0.0, 0.0 ) );
		snsr_T_optical.setRotation( tf::Quaternion( -0.5, 0.5, -0.5, 0.5 ) );
		
		tf::Transform map_T_optical = map_T_snsr * snsr_T_optical;

		pcl_ros::transformPointCloud( *view_ptr, *view_ptr, map_T_optical);
		
		// add to pcd_model
		*pcd_model_ptr +=  *view_ptr;
	}
	
	// Save
	pcl::PCDWriter writer;
	writer.writeASCII( vision_module_dir + "../database/pcd_models/" + obj_name + "_complete.pcd", *pcd_model_ptr );
	
	return 0;
}
// this is just a workbench. most of the stuff here will go into the Frontend class.
int main(int argc, char **argv) {

  ros::init(argc, argv, "okvis_node_synchronous");

  google::InitGoogleLogging(argv[0]);
  FLAGS_stderrthreshold = 0; // INFO: 0, WARNING: 1, ERROR: 2, FATAL: 3
  FLAGS_colorlogtostderr = 1;

  if (argc != 3 && argc != 4) {
    LOG(ERROR) <<
        "Usage: ./" << argv[0] << " configuration-yaml-file bag-to-read-from [skip-first-seconds]";
    return -1;
  }

  okvis::Duration deltaT(0.0);
  if (argc == 4) {
    deltaT = okvis::Duration(atof(argv[3]));
  }

  // set up the node
  ros::NodeHandle nh("okvis_node");

  // publisher
  okvis::Publisher publisher(nh);

  // read configuration file
  std::string configFilename(argv[1]);

  okvis::RosParametersReader vio_parameters_reader(configFilename);
  okvis::VioParameters parameters;
  vio_parameters_reader.getParameters(parameters);

  okvis::ThreadedKFVio okvis_estimator(parameters);

  okvis_estimator.setFullStateCallback(std::bind(&okvis::Publisher::publishFullStateAsCallback,&publisher,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4));
  okvis_estimator.setLandmarksCallback(std::bind(&okvis::Publisher::publishLandmarksAsCallback,&publisher,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3));
  okvis_estimator.setStateCallback(std::bind(&okvis::Publisher::publishStateAsCallback,&publisher,std::placeholders::_1,std::placeholders::_2));
  okvis_estimator.setBlocking(true);
  publisher.setParameters(parameters); // pass the specified publishing stuff

  // extract the folder path
  std::string bagname(argv[2]);
  size_t pos = bagname.find_last_of("/");
  std::string path;
  if (pos == std::string::npos)
    path = ".";
  else
    path = bagname.substr(0, pos);

  const unsigned int numCameras = parameters.nCameraSystem.numCameras();

  // setup files to be written
  publisher.setCsvFile(path + "/okvis_estimator_output.csv");
  publisher.setLandmarksCsvFile(path + "/okvis_estimator_landmarks.csv");
  okvis_estimator.setImuCsvFile(path + "/imu0_data.csv");
  for (size_t i = 0; i < numCameras; ++i) {
    std::stringstream num;
    num << i;
    okvis_estimator.setTracksCsvFile(i, path + "/cam" + num.str() + "_tracks.csv");
  }

  // open the bag
  rosbag::Bag bag(argv[2], rosbag::bagmode::Read);
  // views on topics. the slash is needs to be correct, it's ridiculous...
  std::string imu_topic("/imu0");
  rosbag::View view_imu(
      bag,
      rosbag::TopicQuery(imu_topic));
  if (view_imu.size() == 0) {
    LOG(ERROR) << "no imu topic";
    return -1;
  }
  rosbag::View::iterator view_imu_iterator = view_imu.begin();
  LOG(INFO) << "No. IMU messages: " << view_imu.size();

  std::vector<std::shared_ptr<rosbag::View> > view_cams_ptr;
  std::vector<rosbag::View::iterator> view_cam_iterators;
  std::vector<okvis::Time> times;
  okvis::Time latest(0);
  for(size_t i=0; i<numCameras;++i) {
    std::string camera_topic("/cam"+std::to_string(i)+"/image_raw");
    std::shared_ptr<rosbag::View> view_ptr(
        new rosbag::View(
            bag,
            rosbag::TopicQuery(camera_topic)));
    if (view_ptr->size() == 0) {
      LOG(ERROR) << "no camera topic";
      return 1;
    }
    view_cams_ptr.push_back(view_ptr);
    view_cam_iterators.push_back(view_ptr->begin());
    sensor_msgs::ImageConstPtr msg1 = view_cam_iterators[i]
        ->instantiate<sensor_msgs::Image>();
    times.push_back(
        okvis::Time(msg1->header.stamp.sec, msg1->header.stamp.nsec));
    if (times.back() > latest)
      latest = times.back();
    LOG(INFO) << "No. cam " << i << " messages: " << view_cams_ptr.back()->size();
  }

  for(size_t i=0; i<numCameras;++i) {
    if ((latest - times[i]).toSec() > 0.01)
      view_cam_iterators[i]++;
  }

  int counter = 0;
  okvis::Time start(0.0);
  while (ros::ok()) {
    ros::spinOnce();
	okvis_estimator.display();

    // check if at the end
    if (view_imu_iterator == view_imu.end()){
      std::cout << std::endl << "Finished. Press any key to exit." << std::endl << std::flush;
      char k = 0;
      while(k==0 && ros::ok()){
        k = cv::waitKey(1);
        ros::spinOnce();
      }
      return 0;
    }
    for (size_t i = 0; i < numCameras; ++i) {
      if (view_cam_iterators[i] == view_cams_ptr[i]->end()) {
        std::cout << std::endl << "Finished. Press any key to exit." << std::endl << std::flush;
        char k = 0;
        while(k==0 && ros::ok()){
          k = cv::waitKey(1);
          ros::spinOnce();
        }
        return 0;
      }
    }

    // add images
    okvis::Time t;
    for(size_t i=0; i<numCameras;++i) {
      sensor_msgs::ImageConstPtr msg1 = view_cam_iterators[i]
          ->instantiate<sensor_msgs::Image>();
      cv::Mat filtered(msg1->height, msg1->width, CV_8UC1);
      memcpy(filtered.data, &msg1->data[0], msg1->height * msg1->width);
      t = okvis::Time(msg1->header.stamp.sec, msg1->header.stamp.nsec);
      if (start == okvis::Time(0.0)) {
        start = t;
      }

      // get all IMU measurements till then
      okvis::Time t_imu=start;
      do {
        sensor_msgs::ImuConstPtr msg = view_imu_iterator
            ->instantiate<sensor_msgs::Imu>();
        Eigen::Vector3d gyr(msg->angular_velocity.x, msg->angular_velocity.y,
                            msg->angular_velocity.z);
        Eigen::Vector3d acc(msg->linear_acceleration.x,
                            msg->linear_acceleration.y,
                            msg->linear_acceleration.z);

        t_imu = okvis::Time(msg->header.stamp.sec, msg->header.stamp.nsec);

        // add the IMU measurement for (blocking) processing
        if (t_imu - start > deltaT)
          okvis_estimator.addImuMeasurement(t_imu, acc, gyr);

        view_imu_iterator++;
      } while (view_imu_iterator != view_imu.end() && t_imu <= t);

      // add the image to the frontend for (blocking) processing
      if (t - start > deltaT)
        okvis_estimator.addImage(t, i, filtered);

      view_cam_iterators[i]++;
    }
    ++counter;

    // display progress
    if (counter % 20 == 0) {
      std::cout
          << "\rProgress: "
          << int(double(counter) / double(view_cams_ptr.back()->size()) * 100)
          << "%  " ;
    }

  }

  std::cout << std::endl;
  return 0;
}