Exemplo n.º 1
0
int GetImageIndex(const wxString &fileName)
{
  int imageInd = -1;
  wxString fileExt = fileName.AfterLast(wxChar('.'));

#ifdef _WIN32
  // Special case for windows executables, icon and cursor files.
  if (fileExt.CmpNoCase(wxT("exe")) == 0 || fileExt.CmpNoCase(wxT("ico")) == 0 || fileExt.CmpNoCase(wxT("cur")) == 0)
  {
    // Do we already have an icon for the file?
    ImageMap::iterator it = g_imageMap.find(fileName);
    if (it != g_imageMap.end())
    {
      // Use exsiting icon.
      imageInd = it->second;
    }
    else
    {
      wxString fullname = fileName;
      if (fileExt.CmpNoCase(wxT("exe")) == 0)
        fullname += wxT(";0"); // Icon index.

      // Try to load 16x16 icon. Load any icon if fails.
      wxIcon icon;
      if (!icon.LoadFile(fullname, wxBITMAP_TYPE_ICO, 16, 16))
        icon.LoadFile(fullname, wxBITMAP_TYPE_ICO);

      // Add the icon into the list.
      if (icon.Ok())
      {
        if (fileExt.CmpNoCase(wxT("cur")) != 0)
        {
          // Convert to bitmap. It fixes the alpha problem.
          wxBitmap bmp(icon);
          if (bmp.Ok())
            imageInd = g_imageList->Add(bmp);
        }
        else
        {
          // Can't convert cursor to bitmap.
          imageInd = g_imageList->Add(icon);
        }
      }

      if (imageInd >= 0)
        g_imageMap.insert(ImagePair(fileName, imageInd));
    }
  }
#endif // _WIN32

  if (imageInd < 0 && fileExt.Length() > 0)
  {
    // Do we already have an icon for the file type?
    ImageMap::iterator it = g_imageMap.find(fileExt);
    if (it != g_imageMap.end())
    {
      // Use exsiting icon.
      imageInd = it->second;
    }
    else
    {
      // Ask mime types manager for the file type.
      // TODO: For some types (sdf, sln) it returns wrong index for the icon. Fix it.
      // TODO: For some types (doc, html) it returns invalid file name for the icon. Fix it.
      wxFileType *fileType = wxTheMimeTypesManager->GetFileTypeFromExtension(fileExt);
      if (fileType != NULL)
      {
        // Get icon location.
        wxIconLocation iconLoc;
        if (fileType->GetIcon(&iconLoc))
        {
          wxString fullname = iconLoc.GetFileName();
#ifdef _WIN32
          if (iconLoc.GetIndex())
            fullname << wxT(';') << iconLoc.GetIndex();
#endif

          // Try to load 16x16 icon. Load any icon if fails.
          wxIcon icon;
          if (!icon.LoadFile(fullname, wxBITMAP_TYPE_ICO, 16, 16))
            icon.LoadFile(fullname, wxBITMAP_TYPE_ICO);

          // Add the icon into the list.
          if (icon.Ok())
          {
            // Convert to bitmap. It fixes the alpha problem.
            wxBitmap bmp(icon);
            if (bmp.Ok())
              imageInd = g_imageList->Add(bmp);
          }

          if (imageInd >= 0)
            g_imageMap.insert(ImagePair(fileExt, imageInd));
        }

        delete fileType;
      }
    }
  }

  // If no image found.
  if (imageInd < 0)
  {
    // Set default image.
    g_imageMap.insert(ImagePair(fileExt, kUnknownFileImage));
    imageInd = kUnknownFileImage;
  }

  return imageInd;
}
Exemplo n.º 2
0
/* Quickly compute pose of all cameras */
void BundlerApp::BundleAdjustFast() 
{
    clock_t start = clock();

    /* Compute initial image information */
    ComputeGeometricConstraints();//读取constrains.txt里面的信息,构造track信息

#if 0
    /* Read keypoints */
    printf("[BundleAdjust] Reading key colors...\n");
    ReadKeyColors();
#endif

    /* Set track pointers to -1 */
    for (int i = 0; i < (int) m_track_data.size(); i++) 
	{
		m_track_data[i].m_extra = -1;
    }

    /* For now, assume all images form one connected component */
    int num_images = GetNumImages();
    int *added_order = new int[num_images];
    int *added_order_inv = new int[num_images];


    /* **** Run bundle adjustment! **** */

    camera_params_t *cameras = new camera_params_t[num_images];
    int max_pts = (int) m_track_data.size(); // 1243742; /* HACK! */
    v3_t *points = new v3_t[max_pts];
    v3_t *colors = new v3_t[max_pts];
    std::vector<ImageKeyVector> pt_views;


    /* Initialize the bundle adjustment */
    int num_init_cams = 0;
    InitializeBundleAdjust(num_init_cams, added_order, added_order_inv,
			   cameras, points, colors, pt_views, 
			   m_use_constraints);

    int i_best = -1, j_best = -1, max_matches = 0;
    double max_score = 0.0;
    int curr_num_cameras, curr_num_pts;
    int pt_count;

    if (num_init_cams == 0) 
	{
		BundlePickInitialPair(i_best, j_best, true);

		added_order[0] = i_best;
		added_order[1] = j_best;

		printf("[BundleAdjust] Adjusting cameras "
			   "%d and %d (score = %0.3f)\n", 
			   i_best, j_best, max_score);

		/* **** Set up the initial cameras **** */
		double init_focal_length_0 = 0.0, init_focal_length_1 = 0.0;
		pt_count = curr_num_pts = 
	    SetupInitialCameraPair(i_best, j_best, 
				   init_focal_length_0, init_focal_length_1,
				   cameras, points, colors, pt_views);

        DumpOutputFile(m_output_directory, "bundle.init.out",
                       num_images, 2, curr_num_pts,
                       added_order, cameras, points, colors, pt_views);

		/* Run sfm for the first time */
		double error0;
		error0 = RunSFM(curr_num_pts, 2, 0, false,
				cameras, points, added_order, colors, pt_views);

		printf("  focal lengths: %0.3f, %0.3f\n", cameras[0].f, cameras[1].f);
#if 1
		if (m_fix_necker) {
			/* Swap the cameras and flip the depths to deal with Necker
			 * reversal */

			camera_params_t cameras_old[2];
			v3_t *points_old;

			points_old = new v3_t [curr_num_pts];

			memcpy(points_old, points, sizeof(v3_t) * curr_num_pts);
			memcpy(cameras_old, cameras, sizeof(camera_params_t) * 2);

			camera_params_t tmp = cameras[0];
			memcpy(cameras[0].R, cameras[1].R, sizeof(double) * 9);
			memcpy(cameras[0].t, cameras[1].t, sizeof(double) * 3);
			cameras[0].f = init_focal_length_0;
			cameras[0].k[0] = cameras[0].k[1] = 0.0;

			memcpy(cameras[1].R, tmp.R, sizeof(double) * 9);
			memcpy(cameras[1].t, tmp.t, sizeof(double) * 3);
			cameras[1].f = init_focal_length_1;
			cameras[1].k[0] = cameras[1].k[1] = 0.0;
	
			double K1inv[9] = 
			{ 1.0 / cameras[0].f, 0.0, 0.0,
			  0.0, 1.0 / cameras[0].f, 0.0,
			  0.0, 0.0, 1.0 };

			double K2inv[9] = 
			{ 1.0 / cameras[1].f, 0.0, 0.0,
			  0.0, 1.0 / cameras[1].f, 0.0,
			  0.0, 0.0, 1.0 };

			for (int i = 0; i < curr_num_pts; i++) {
				int k1 = pt_views[i][0].second;
				int k2 = pt_views[i][1].second;

				double proj1[3] = { GetKey(added_order[0],k1).m_x,
							GetKey(added_order[0],k1).m_y,
							-1.0 };
		
				double proj2[3] = { GetKey(added_order[1],k2).m_x,
							GetKey(added_order[1],k2).m_y,
							-1.0 };

				double proj1_norm[3], proj2_norm[3];
		
				matrix_product(3, 3, 3, 1, K1inv, proj1, proj1_norm);
				matrix_product(3, 3, 3, 1, K2inv, proj2, proj2_norm);

				v2_t p = v2_new(proj1_norm[0] / proj1_norm[2],
						proj1_norm[1] / proj1_norm[2]);
	    
				v2_t q = v2_new(proj2_norm[0] / proj2_norm[2],
						proj2_norm[1] / proj2_norm[2]);

				double proj_error;
		
				double t1[3];
				double t2[3];
			
				/* Put the translation in standard form */
				matrix_product(3, 3, 3, 1, cameras[0].R, cameras[0].t, t1);
				matrix_scale(3, 1, t1, -1.0, t1);
				matrix_product(3, 3, 3, 1, cameras[1].R, cameras[1].t, t2);
				matrix_scale(3, 1, t2, -1.0, t2);
		
				points[i] = triangulate(p, q, 
										cameras[0].R, t1, cameras[1].R, t2,
										&proj_error);
			}

			double error1;
			error1 = RunSFM(curr_num_pts, 2, 0, false,
					cameras, points, added_order, colors, pt_views);
		}
#endif
		DumpPointsToPly(m_output_directory, "points001.ply", 
                        curr_num_pts, 2, points, colors, cameras);

		if (m_bundle_output_base != NULL) {
			char buf[256];
			sprintf(buf, "%s%03d.out", m_bundle_output_base, 1);
			DumpOutputFile(m_output_directory, buf, num_images, 2, curr_num_pts,
				   added_order, cameras, points, colors, pt_views);
		}

		curr_num_cameras = 2;
    } 
	else {
		curr_num_cameras = num_init_cams;
		pt_count = curr_num_pts = (int) m_point_data.size();
    }
    
    int round = 0;
    while (curr_num_cameras < num_images) {
		int parent_idx;
		int max_cam = 
            FindCameraWithMostMatches(curr_num_cameras, curr_num_pts, 
                                      added_order, parent_idx, 
                                      max_matches, pt_views);

		printf("[BundleAdjust] max_matches = %d\n", max_matches);

		if (max_matches < m_min_max_matches)
	    break; /* No more connections */

		/* Find all images with 75% of the matches of the maximum 
         * (unless overruled by m_num_points_add_camera) */
		std::vector<ImagePair> image_set;

        if (false && max_matches < 48) { /* disabling this */
            image_set.push_back(ImagePair(max_cam, parent_idx));
        } else {
            int nMatches = iround(0.75 * max_matches);

            if (m_num_matches_add_camera > 0) {
                /* Alternate threshold based on user parameter */
                nMatches = std::min(nMatches, m_num_matches_add_camera);
            }

			image_set = 
                FindCamerasWithNMatches(nMatches,
                                        curr_num_cameras, curr_num_pts, 
                                        added_order, pt_views);
        }
        
		int num_added_images = (int) image_set.size();

		printf("[BundleAdjustFast] Registering %d images\n",
			   num_added_images);

		for (int i = 0; i < num_added_images; i++)
			printf("[BundleAdjustFast] Adjusting camera %d\n",
			   image_set[i].first);

	/* Now, throw the new cameras into the mix */
    int image_count = 0;
	for (int i = 0; i < num_added_images; i++) {
	    int next_idx = image_set[i].first;
	    int parent_idx = image_set[i].second;

	    added_order[curr_num_cameras + image_count] = next_idx;

	    printf("[BundleAdjust[%d]] Adjusting camera %d "
		   "(parent = %d)\n", 
		   round, next_idx, 
                   (parent_idx == -1 ? -1 : added_order[parent_idx]));

	    /* **** Set up the new camera **** */
        bool success = false;
        camera_params_t camera_new = 
		BundleInitializeImage(m_image_data[next_idx], 
				    next_idx, 
                    curr_num_cameras + image_count,
				    curr_num_cameras, curr_num_pts,
				    added_order, points, 
				    NULL, cameras, 
				    pt_views, &success);

        if (success) {
            cameras[curr_num_cameras+image_count] = camera_new;
            image_count++;
        } else {
            printf("[BundleAdjust] Couldn't initialize image %d\n",
                    next_idx);
            m_image_data[next_idx].m_ignore_in_bundle = true;
        }
	}
	

	/* Compute the distance between the first pair of cameras */
    double dist0 = 0.0;
	printf("[BundleAdjust] Adding new matches\n");

	pt_count = curr_num_pts;
	
	curr_num_cameras += image_count;

        if (!m_skip_add_points) {
            pt_count = 
                BundleAdjustAddAllNewPoints(pt_count, curr_num_cameras,
                                            added_order, cameras, 
                                            points, colors,
                                            dist0, pt_views);
        }
        
	curr_num_pts = pt_count;

	printf("[BundleAdjust] Number of points = %d\n", pt_count);
	fflush(stdout);

        if (!m_skip_full_bundle) {
            /* Run sfm again to update parameters */
            RunSFM(curr_num_pts, curr_num_cameras, 0, false,
                   cameras, points, added_order, colors, pt_views);

            /* Remove bad points and cameras */
            RemoveBadPointsAndCameras(curr_num_pts, curr_num_cameras + 1, 
                                      added_order, cameras, points, colors, 
                                      pt_views);

            printf("  focal lengths:\n");
	
            for (int i = 0; i < curr_num_cameras; i++) {
                if(m_image_data[added_order[i]].m_has_init_focal) {
                    printf("   [%03d] %0.3f (%0.3f) %s %d; %0.3e %0.3e\n", 
                           i, cameras[i].f, 
                           m_image_data[added_order[i]].m_init_focal,
                           m_image_data[added_order[i]].m_name,
                           added_order[i], cameras[i].k[0], cameras[i].k[1]);
                } else {
                    printf("   [%03d] %0.3f %s %d; %0.3e %0.3e\n", 
                           i, cameras[i].f, 
                           m_image_data[added_order[i]].m_name,
                           added_order[i], cameras[i].k[0], cameras[i].k[1]);
                }
                // printf("   [%03d] %0.3f\n", i, cameras[i].f);
            }

            fflush(stdout);
        }

	/* Dump output for this round */
	char buf[256];
	sprintf(buf, "points%03d.ply", curr_num_cameras);

	DumpPointsToPly(m_output_directory, buf, 
                        curr_num_pts, curr_num_cameras, 
                        points, colors, cameras);

	if (m_bundle_output_base != NULL) {
	    sprintf(buf, "%s%03d.out", m_bundle_output_base, 
                    curr_num_cameras);
	    DumpOutputFile(m_output_directory, buf, 
                           num_images, curr_num_cameras, curr_num_pts,
			   added_order, cameras, points, colors, pt_views);
	}

	round++;
    }//while

    clock_t end = clock();

    printf("[BundleAdjust] Bundle adjustment took %0.3fs\n",
	   (end - start) / ((double) CLOCKS_PER_SEC));

    if (m_estimate_ignored) {
        EstimateIgnoredCameras(curr_num_cameras,
                               cameras, added_order,
                               curr_num_pts, points, colors, pt_views);
    }

    /* Dump output */
    if (m_bundle_output_file != NULL) {
	DumpOutputFile(m_output_directory, m_bundle_output_file, 
		       num_images, curr_num_cameras, curr_num_pts,
		       added_order, cameras, points, colors, pt_views);
    }

    /* Save the camera parameters and points */

    /* Cameras */
    for (int i = 0; i < num_images; i++) {
	m_image_data[i].m_camera.m_adjusted = false;
    }

    for (int i = 0; i < curr_num_cameras; i++) {
		int img = added_order[i];

		m_image_data[img].m_camera.m_adjusted = true;
		memcpy(m_image_data[img].m_camera.m_R, cameras[i].R, 
			   9 * sizeof(double));

        matrix_product(3, 3, 3, 1, 
                       cameras[i].R, cameras[i].t,
                       m_image_data[img].m_camera.m_t);

        matrix_scale(3, 1, 
                     m_image_data[img].m_camera.m_t, -1.0, 
                     m_image_data[img].m_camera.m_t);	    

		m_image_data[img].m_camera.m_focal = cameras[i].f;

		m_image_data[img].m_camera.Finalize();
		}

    /* Points */
    for (int i = 0; i < curr_num_pts; i++) {
	/* Check if the point is visible in any view */
	if ((int) pt_views[i].size() == 0) 
	    continue; /* Invisible */
	
	PointData pdata;
	pdata.m_pos[0] = Vx(points[i]);
	pdata.m_pos[1] = Vy(points[i]);
	pdata.m_pos[2] = Vz(points[i]);

	pdata.m_color[0] = (float) Vx(colors[i]);
	pdata.m_color[1] = (float) Vy(colors[i]);
	pdata.m_color[2] = (float) Vz(colors[i]);

#if 1
	for (int j = 0; j < (int) pt_views[i].size(); j++) {
	    int v = pt_views[i][j].first;
	    int vnew = added_order[v];
	    pdata.m_views.push_back(ImageKey(vnew, pt_views[i][j].second));
	}
#else
	pdata.m_views = pt_views[i];
#endif

	m_point_data.push_back(pdata);
    }

    delete [] added_order;
    delete [] added_order_inv;

    SetMatchesFromPoints();

    bool *image_mask = new bool[num_images];

    for (int i = 0; i < num_images; i++) {
	if (m_image_data[i].m_camera.m_adjusted)
	    image_mask[i] = true;
	else 
	    image_mask[i] = false;
    }
}