pangolin-task6

Task6

basalt 中 basalt_calibrate 的 pangolin gui

按基本的工作流，解析下面的代码

• 入口

basalt::CamCalib cv(dataset_path, dataset_type, aprilgrid_path, result_path, 
		cache_dataset_name, skip_images, cam_types);

cv.renderingLoop();

• 构造函数

CamCalib::CamCalib(..., bool show_gui): 
//...
show_gui(show_gui), 							// 是否使用gui
show_frame("ui.show_frame", 0, 0, 1500), 		// 滑条，显示第几幅图像
show_corners("ui.show_corners", true, false, true), // 复选框，是否显示角点
show_corners_rejected("ui.show_corners_rejected", false, false, true), // 复选框，显示质量不好的角点
show_init_reproj("ui.show_init_reproj", false, false, true), 	// 复选框，显示初始投影矩阵的效果
show_opt("ui.show_opt", true, false, true), 					// 复选框
show_vign("ui.show_vign", false, false, true), 					// 复选框
show_ids("ui.show_ids", false, false, true), 					// 复选框
huber_thresh("ui.huber_thresh", 4.0, 0.1, 10.0), 				// 滑条
opt_intr("ui.opt_intr", true, false, true), 					// 复选框，优化前是否进行初始化参数
opt_until_convg("ui.opt_until_converge", false, false, true), 	// 复选框，是否优化直到收敛
stop_thresh("ui.stop_thresh", 1e - 8, 1e - 10, 0.01, true)		// 指数滑条，停止迭代的阈值
{
	if (show_gui)
		initGui();

	//...

	pangolin::ColourWheel cw;									

	for (int i = 0; i < 20; i++)
	{
		cam_colors.emplace_back(cw.GetUniqueColour());
	}
}

• 初始化gui

void CamCalib::initGui()
{
	pangolin::CreateWindowAndBind("Main", 1600, 1000);

	pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, 
		pangolin::Attach::Pix(UI_WIDTH));

	img_view_display = &pangolin::CreateDisplay().SetBounds(0.5, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0).SetLayout(pangolin::LayoutEqual);

	pangolin::View & vign_plot_display = pangolin::CreateDisplay().SetBounds(0.0, 0.5, 0.72, 1.0);

	vign_plotter.reset(new pangolin::Plotter(&vign_data_log, 0.0, 1000.0, 0.0, 1.0, 0.01f, 0.01f));
	vign_plot_display.AddDisplay(*vign_plotter);

	pangolin::View & polar_error_display = pangolin::CreateDisplay().SetBounds(0.0, 0.5, pangolin::Attach::Pix(UI_WIDTH), 0.43);

	polar_plotter.reset(new pangolin::Plotter(nullptr, 0.0, 120.0, 0.0, 1.0, 0.01f, 0.01f));
	polar_error_display.AddDisplay(*polar_plotter);

	pangolin::View & azimuthal_plot_display = pangolin::CreateDisplay().SetBounds(0.0, 0.5, 0.45, 0.7);

	azimuth_plotter.reset(new pangolin::Plotter(nullptr, -180.0, 180.0, 0.0, 1.0, 0.01f, 0.01f));
	azimuthal_plot_display.AddDisplay(*azimuth_plotter);

	pangolin::Var < std::function < void(void) >> load_dataset("ui.load_dataset", 
		std::bind(&CamCalib::loadDataset, this));

	pangolin::Var < std::function < void(void) >> detect_corners("ui.detect_corners", 
		std::bind(&CamCalib::detectCorners, this));

	pangolin::Var < std::function < void(void) >> init_cam_intrinsics("ui.init_cam_intr", 
		std::bind(&CamCalib::initCamIntrinsics, this));

	pangolin::Var < std::function < void(void) >> init_cam_poses("ui.init_cam_poses", 
		std::bind(&CamCalib::initCamPoses, this));

	pangolin::Var < std::function < void(void) >> init_cam_extrinsics("ui.init_cam_extr", 
		std::bind(&CamCalib::initCamExtrinsics, this));

	pangolin::Var < std::function < void(void) >> init_opt("ui.init_opt", 
		std::bind(&CamCalib::initOptimization, this));

	pangolin::Var < std::function < void(void) >> optimize("ui.optimize", std::bind(&CamCalib::optimize, this));

	pangolin::Var < std::function < void(void) >> save_calib("ui.save_calib", 
		std::bind(&CamCalib::saveCalib, this));

	pangolin::Var < std::function < void(void) >> compute_vign("ui.compute_vign", 
		std::bind(&CamCalib::computeVign, this));

	setNumCameras(1);
}

• 相机视窗

void CamCalib::setNumCameras(size_t n)
{
	while (img_view.size() < n && show_gui)
	{
		std::shared_ptr < pangolin::ImageView > iv(new pangolin::ImageView);

		size_t			idx = img_view.size();

		img_view.push_back(iv);

		img_view_display->AddDisplay(*iv);
		iv->extern_draw_function = std::bind(&CamCalib::drawImageOverlay, this, 
			std::placeholders::_1, idx);
	}
}

• 转起来

void CamCalib::renderingLoop()
{
	while (!pangolin::ShouldQuit())
	{
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		if (vio_dataset.get())
		{
			if (show_frame.GuiChanged())
			{
				size_t			frame_id = static_cast < size_t > (show_frame);
				int64_t 		timestamp = vio_dataset->get_image_timestamps()[frame_id];

				const std::vector < ImageData > &img_vec = vio_dataset->get_image_data(timestamp);

				for (size_t cam_id = 0; cam_id < vio_dataset->get_num_cams(); cam_id++)
					if (img_vec[cam_id].img.get())
					{
						pangolin::GlPixFormat fmt;
						fmt.glformat		= GL_LUMINANCE;
						fmt.gltype			= GL_UNSIGNED_SHORT;
						fmt.scalable_internal_format = GL_LUMINANCE16;

						img_view[cam_id]->SetImage(img_vec[cam_id].img->ptr, img_vec[cam_id].img->w, img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt);
					}
				else 
				{
					img_view[cam_id]->Clear();
				}
			}
		}

		// 优化

		pangolin::FinishFrame();
	}
}

逐行解析

仅看感兴趣的

// CamCalib::CamCalib
pangolin::ColourWheel cw;									

for (int i = 0; i < 20; i++)
{
	cam_colors.emplace_back(cw.GetUniqueColour());
}

ColourWheel 是相当于一个调色板，通过 GetUniqueColour 从中提取不同的颜色。

img_view_display = &pangolin::CreateDisplay().SetBounds(0.5, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0).SetLayout(pangolin::LayoutEqual);

//...

setNumCameras(1);

//...

void CamCalib::setNumCameras(size_t n)
{
	while (img_view.size() < n && show_gui)
	{
		std::shared_ptr < pangolin::ImageView > iv(new pangolin::ImageView);

		size_t			idx = img_view.size();

		img_view.push_back(iv);

		img_view_display->AddDisplay(*iv);
		iv->extern_draw_function = std::bind(&CamCalib::drawImageOverlay, this, 
			std::placeholders::_1, idx);
	}
}

img_view_display 相机视窗，里面可能包括多个相机，因此采用equal自动布局。

为每个相机创建pangolin::ImageView窗口，将它们加到img_view_display视窗里面，并为每个窗口设置绘图程序。这里涉及到 drawImageOverlay 函数，其函数如下：

void CamCalib::drawImageOverlay(pangolin::View & v, size_t cam_id)
{
	// https://baijiahao.baidu.com/s?id=1645466623988106939&wfr=spider&for=pc
	UNUSED(v);
	size_t			frame_id = show_frame;

	if (vio_dataset && frame_id < vio_dataset->get_image_timestamps().size())
	{
		int64_t 		timestamp_ns = vio_dataset->get_image_timestamps()[frame_id];
		TimeCamId tcid( timestamp_ns, cam_id);

		if (show_corners)
		{
			glLineWidth(1.0);
			glColor3f(1.0, 0.0, 0.0);
			glEnable(GL_BLEND);
			glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

			if (calib_corners.find(tcid) != calib_corners.end())
			{
				const CalibCornerData & cr = calib_corners.at(tcid);
				const CalibCornerData & cr_rej = calib_corners_rejected.at(tcid);

				for (size_t i = 0; i < cr.corners.size(); i++)
				{
					// the radius is the threshold used for maximum displacement. The
					// search region is slightly larger.
					const float 	radius = static_cast <float> (cr.radii[i]);
					const Eigen::Vector2f c = cr.corners[i].cast <float> ();

					pangolin::glDrawCirclePerimeter(c[0], c[1], radius);

					if (show_ids)
						pangolin::GlFont::I().Text("%d", cr.corner_ids[i]).Draw(c[0], c[1]);
				}

				
//...

drawImageOverlay 会在 ImageView::Render 中调用。通过提取角点坐标，用glDrawCirclePerimeter函数画圈圈到窗口上去。这里会不会涉及图像坐标系和窗口坐标系的变换呢？还用GlFont::I().Text打印了一行文字。

相机图像的绘制在主循环renderingLoop中进行，如下

// CamCalib::renderingLoop
pangolin::GlPixFormat fmt;
fmt.glformat		= GL_LUMINANCE;
fmt.gltype			= GL_UNSIGNED_SHORT;
fmt.scalable_internal_format = GL_LUMINANCE16;

img_view[cam_id]->SetImage(img_vec[cam_id].img->ptr, img_vec[cam_id].img->w, img_vec[cam_id].img->h, img_vec[cam_id].img->pitch, fmt);

从数据集中提取图像，更新到对应相机窗口中。

• GL_LUMINANCE 按照亮度值存储纹理单元
• GL_UNSIGNED_SHORT 无符号短整形
• GL_LUMINANCE16 16位亮度值

下面看一下vign_plotter、polar_plotter、azimuth_plotter的使用，它们的定义如下：

pangolin::View & vign_plot_display = pangolin::CreateDisplay().SetBounds(0.0, 0.5, 0.72, 1.0);

vign_plotter.reset(new pangolin::Plotter(&vign_data_log, 0.0, 1000.0, 0.0, 1.0, 0.01f, 0.01f));
vign_plot_display.AddDisplay(*vign_plotter);

pangolin::View & polar_error_display = pangolin::CreateDisplay().SetBounds(0.0, 0.5, pangolin::Attach::Pix(UI_WIDTH), 0.43);

polar_plotter.reset(new pangolin::Plotter(nullptr, 0.0, 120.0, 0.0, 1.0, 0.01f, 0.01f));
polar_error_display.AddDisplay(*polar_plotter);

pangolin::View & azimuthal_plot_display = pangolin::CreateDisplay().SetBounds(0.0, 0.5, 0.45, 0.7);

azimuth_plotter.reset(new pangolin::Plotter(nullptr, -180.0, 180.0, 0.0, 1.0, 0.01f, 0.01f));
azimuthal_plot_display.AddDisplay(*azimuth_plotter);

//...

pangolin::Var < std::function < void(void) >> compute_vign("ui.compute_vign", 
			std::bind(&CamCalib::computeVign, this));

vign_plotter 使用了数据容器 vign_data_log，并在 computeVign 中使用。给computeVign添加了一个按钮控件，来控制执行。

// CamCalib::computeProjections
while (polar_data_log.size() < calib_opt->calib->intrinsics.size())
{
	polar_data_log.emplace_back(new pangolin::DataLog);
}

while (azimuth_data_log.size() < calib_opt->calib->intrinsics.size())
{
	azimuth_data_log.emplace_back(new pangolin::DataLog);
}

constexpr int	MIN_POINTS_HIST = 3;

polar_plotter->ClearSeries();
azimuth_plotter->ClearSeries();

for (size_t c = 0; c < calib_opt->calib->intrinsics.size(); c++)
{
	polar_data_log[c]->Clear();
	azimuth_data_log[c]->Clear();

	for (int i = 0; i < polar_sum[c].rows(); i++)
	{
		if (polar_num[c][i] > MIN_POINTS_HIST)
		{
			double			x_coord = ANGLE_BIN_SIZE * i + ANGLE_BIN_SIZE / 2.0;
			double			mean_reproj = polar_sum[c][i] / polar_num[c][i];

			polar_data_log[c]->Log(x_coord, mean_reproj);
		}
	}

	polar_plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, cam_colors[c], 
		"mean error(pix) vs polar angle(deg) for cam" + std::to_string(c), 
		polar_data_log[c].get());

	for (int i = 0; i < azimuth_sum[c].rows(); i++)
	{
		if (azimuth_num[c][i] > MIN_POINTS_HIST)
		{
			double			x_coord = ANGLE_BIN_SIZE * i + ANGLE_BIN_SIZE / 2.0 - 180.0;
			double			mean_reproj = azimuth_sum[c][i] / azimuth_num[c][i];

			azimuth_data_log[c]->Log(x_coord, mean_reproj);
		}
	}

	azimuth_plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, cam_colors[c], 
		"mean error(pix) vs azimuth angle(deg) for cam" + std::to_string(c), 
		azimuth_data_log[c].get());
}

polar_plotter、azimuth_plotter 分别用了 polar_data_log、azimuth_data_log 这两个数据容器，来绘制误差图。

总结

pangolin::ImageView 继承了View 和 Handler，因此可以设置交互。