Point3DVector LoadPoints3D(const std::string& filename)
{
  std::cout << "LoadPoint3D " << filename << std::endl;
  std::string line;
  std::ifstream fin(filename.c_str());
  Point3DVector points;
  if(fin == NULL)
    {
    std::stringstream ss;
    ss << "CameraCalibration:LoadPoints3D Cannot open file " << filename;
    throw std::runtime_error(ss.str());
    }

  while(getline(fin, line))
    {
    std::stringstream ss;
    ss << line;
    double p[3];
    ss >> p[0] >> p[1] >> p[2];
    points.push_back(Eigen::Vector3d (p[0], p[1], p[2]));
    }
  return points;
}