class pixelSelector {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    ~pixelSelector() {
	delete[] random_pattern;
	delete[] grad_hist;
	delete[] ths;
	delete[] ths_smoothed;
}
    int pixelSelectionLeft(std::shared_ptr<frame> fh, float density, int recursions_left = 1, float th_factor = 1) {
	float num_have = 0;
	float num_want = density;
	float quotia;
	int ideal_potential = current_potential;

	// if(fh != grad_hist_frame) makeHistsLeft(fh);
	makeHistsLeft(fh);

	Eigen::Vector3i n = this->selectLeft(fh, current_potential, th_factor);

	num_have = n[0] + n[1] + n[2];
	quotia = num_want / num_have;

	float K = num_have * (current_potential + 1) * (current_potential + 1);
	ideal_potential = sqrtf(K / num_want) - 1;

	if (ideal_potential < 1) ideal_potential = 1;

	if (recursions_left > 0 && quotia > 1.25 && current_potential > 1)
	{
		if(ideal_potential >= current_potential)
			ideal_potential = current_potential - 1;

		current_potential = ideal_potential;
		return pixelSelectionLeft(fh, density, recursions_left - 1, th_factor);
	}
	else if(recursions_left > 0 && quotia < 0.25)
	{
		if(ideal_potential <= current_potential)
			ideal_potential = current_potential + 1;

		current_potential = ideal_potential;
		return pixelSelectionLeft(fh, density, recursions_left - 1, th_factor);
	}

	int num_have_sub = num_have;

	if(quotia < 0.95)
	{
		int wh = wG[0] * hG[0];
		int rn = 0;
		unsigned char char_th = 255*quotia;
		for(int i = 0; i < wh; i++)
		{
			if(fh->selected_pixels_left[i] != 0)
			{
				if(random_pattern[rn] > char_th )
				{
					fh->selected_pixels_left[i] = 0;
					num_have_sub--;
				}
				rn++;
			}
		}
	}

	current_potential = ideal_potential;

	return num_have_sub;
}
    int pixelSelectionRight(std::shared_ptr<frame> fh, float density, int recursions_left = 1, float th_factor = 1) {
	float num_have = 0;
	float num_want = density;
	float quotia;
	int ideal_potential = current_potential;

	// if(fh != grad_hist_frame) makeHistsRight(fh);
	makeHistsRight(fh);

	Eigen::Vector3i n = this->selectRight(fh, current_potential, th_factor);

	num_have = n[0] + n[1] + n[2];
	quotia = num_want / num_have;

	float K = num_have * (current_potential + 1) * (current_potential + 1);
	ideal_potential = sqrtf(K / num_want) - 1;

	if (ideal_potential < 1) ideal_potential = 1;

	if (recursions_left > 0 && quotia > 1.25 && current_potential > 1)
	{
		if(ideal_potential >= current_potential)
			ideal_potential = current_potential - 1;

		current_potential = ideal_potential;
		return pixelSelectionRight(fh, density, recursions_left - 1, th_factor);
	}
	else if (recursions_left > 0 && quotia < 0.25)
	{
		if(ideal_potential <= current_potential)
			ideal_potential = current_potential + 1;

		current_potential = ideal_potential;
		return pixelSelectionRight(fh, density, recursions_left - 1, th_factor);
	}

	int num_have_sub = num_have;

	if (quotia < 0.95)
	{
		int wh = wG[0] * hG[0];
		int rn = 0;
		unsigned char char_th = 255*quotia;
		for (int i = 0; i < wh; i++)
		{
			if (fh->selected_pixels_right[i] != 0)
			{
				if (random_pattern[rn] > char_th)
				{
					fh->selected_pixels_right[i] = 0;
					num_have_sub--;
				}
				rn++;
			}
		}
	}

	current_potential = ideal_potential;

	return num_have_sub;
}
  private:
    void makeHistsLeft(std::shared_ptr<frame> fh) {
	grad_hist_frame = fh;
	float* mapmax_0 = fh->abs_squared_grad_left[0];

	int w = wG[0];
	int h = hG[0];

	int w32 = w / 32;
	int h32 = h / 32;
	ths_step = w32;

	for(int y = 0; y < h32; y++)
		for(int x = 0; x < w32; x++)
		{
			float* map_0 = mapmax_0 + 32 * x + 32 * y * w;
			int* hist_0 = grad_hist;

			memset(hist_0, 0, sizeof(int) * 50);

			for(int j = 0; j < 32; j++)
				for(int i = 0; i < 32; i++)
				{
					int it = i + 32 * x;
					int jt = j + 32 * y;

					if(it > w - 2 || jt > h - 2 || it < 1 || jt < 1) continue;

					int g = sqrtf(map_0[i + j * w]);

					if(g > 48) g = 48;

					hist_0[g + 1]++;
					hist_0[0]++;
				}

			ths[x + y * w32] = computeHistQuantil(hist_0, setting_min_grad_hist_cut) + setting_min_grad_hist_add;
		}

	for(int y = 0; y < h32; y++)
		for(int x = 0; x < w32; x++)
		{
			float sum = 0, num = 0;
			if(x > 0)
			{
				if(y > 0)
				{
					num++;
					sum += ths[x - 1 + (y - 1) * w32];
				}

				if(y < h32 - 1)
				{
					num++;
					sum += ths[x - 1 + (y + 1) * w32];
				}

				num++;
				sum += ths[x - 1 + (y) * w32];
			}

			if(x < w32 - 1)
			{
				if(y > 0)
				{
					num++;
					sum += ths[x + 1 + (y - 1) * w32];
				}
				if(y < h32 - 1)
				{
					num++;
					sum += ths[x + 1 + (y + 1) * w32];
				}

				num++;
				sum += ths[x + 1 + (y) * w32];
			}

			if(y > 0)
			{
				num++;
				sum += ths[x + (y - 1) * w32];
			}

			if(y < h32 - 1)
			{
				num++;
				sum += ths[x + (y + 1) * w32];
			}

			num++;
			sum += ths[x + y * w32];

			ths_smoothed[x + y * w32] = (sum / num) * (sum / num);
		}
}
    void makeHistsRight(std::shared_ptr<frame> fh) {
	grad_hist_frame = fh;
	float* mapmax_0 = fh->abs_squared_grad_right[0];

	int w = wG[0];
	int h = hG[0];

	int w32 = w / 32;
	int h32 = h / 32;
	ths_step = w32;

	for(int y = 0; y < h32; y++)
		for(int x = 0; x < w32; x++)
		{
			float* map_0 = mapmax_0 + 32 * x + 32 * y * w;
			int* hist_0 = grad_hist;

			memset(hist_0, 0, sizeof(int) * 50);

			for(int j = 0; j < 32; j++)
				for(int i = 0; i < 32; i++)
				{
					int it = i + 32 * x;
					int jt = j + 32 * y;

					if(it > w - 2 || jt > h - 2 || it < 1 || jt < 1) continue;

					int g = sqrtf(map_0[i + j * w]);

					if(g > 48) g = 48;

					hist_0[g + 1]++;
					hist_0[0]++;
				}

			ths[x + y * w32] = computeHistQuantil(hist_0, setting_min_grad_hist_cut) + setting_min_grad_hist_add;
		}

	for(int y = 0; y < h32; y++)
		for(int x = 0; x < w32; x++)
		{
			float sum = 0, num = 0;

			if(x > 0)
			{
				if(y > 0)
				{
					num++;
					sum += ths[x - 1 + (y - 1) * w32];
				}

				if(y < h32 - 1)
				{
					num++;
					sum += ths[x - 1 + (y + 1) * w32];
				}

				num++;
				sum += ths[x - 1 + (y) * w32];
			}

			if(x < w32 - 1)
			{
				if(y > 0)
				{
					num++;
					sum += ths[x + 1 + (y - 1) * w32];
				}
				if(y < h32 - 1)
				{
					num++;
					sum += ths[x + 1 + (y + 1) * w32];
				}

				num++;
				sum += ths[x + 1 + (y) * w32];
			}

			if(y > 0)
			{
				num++;
				sum += ths[x + (y - 1) * w32];
			}

			if(y < h32 - 1)
			{
				num++;
				sum += ths[x + (y + 1) * w32];
			}

			num++;
			sum += ths[x + y * w32];

			ths_smoothed[x + y * w32] = (sum / num) * (sum / num);
		}
}
    Eigen::Vector3i selectLeft(std::shared_ptr<frame> fh, int pot, float th_factor = 1) {
	Eigen::Vector3f const * const map_0 = fh->dI_left;

	float* mapmax_0 = fh->abs_squared_grad_left[0];
	float* mapmax_1 = fh->abs_squared_grad_left[1];
	float* mapmax_2 = fh->abs_squared_grad_left[2];

	int w = wG[0];
	int w1 = wG[1];
	int w2 = wG[2];
	int h = hG[0];

	const Eigen::Matrix<float, 2, 1> directions[16] = {
		Eigen::Matrix<float, 2, 1>(0, 1.0000),
		Eigen::Matrix<float, 2, 1>(0.3827, 0.9239),
		Eigen::Matrix<float, 2, 1>(0.1951, 0.9808),
		Eigen::Matrix<float, 2, 1>(0.9239, 0.3827),
		Eigen::Matrix<float, 2, 1>(0.7071, 0.7071),
		Eigen::Matrix<float, 2, 1>(0.3827, -0.9239),
		Eigen::Matrix<float, 2, 1>(0.8315, 0.5556),
		Eigen::Matrix<float, 2, 1>(0.8315, -0.5556),
		Eigen::Matrix<float, 2, 1>(0.5556, -0.8315),
		Eigen::Matrix<float, 2, 1>(0.9808, 0.1951),
		Eigen::Matrix<float, 2, 1>(0.9239, -0.3827),
		Eigen::Matrix<float, 2, 1>(0.7071, -0.7071),
		Eigen::Matrix<float, 2, 1>(0.5556, 0.8315),
		Eigen::Matrix<float, 2, 1>(0.9808, -0.1951),
		Eigen::Matrix<float, 2, 1>(1.0000, 0.0000),
		Eigen::Matrix<float, 2, 1>(0.1951, -0.9808)};

	memset(fh->selected_pixels_left, 0, w * h * sizeof(PixelSelectorStatus));

	float dw1 = setting_grad_down_weight_per_level;
	float dw2 = dw1 * dw1;

	int n3 = 0, n2 = 0, n4 = 0;

	for(int y4 = 0; y4 < h; y4 += (4 * pot))
	{
		for(int x4 = 0; x4 < w; x4 += (4 * pot))
		{
			int my3 = std::min((4 * pot), h - y4);
			int mx3 = std::min((4 * pot), w - x4);
			int best_idx_4 = -1;
			float best_val_4 = 0;

			Eigen::Matrix<float, 2, 1> dir4 = directions[random_pattern[n2] & 0xF];

			for(int y3 = 0; y3 < my3; y3 += (2 * pot))
			{
				for(int x3 = 0; x3 < mx3; x3 += (2 * pot))
				{
					int x34 = x3 + x4;
					int y34 = y3 + y4;
					int my2 = std::min((2 * pot), h - y34);
					int mx2 = std::min((2 * pot), w - x34);

					int best_idx_3 = -1;
					float best_val_3 = 0;

					Eigen::Matrix<float, 2, 1> dir3 = directions[random_pattern[n2] & 0xF];

					for(int y2 = 0; y2 < my2; y2 += pot)
					{
						for(int x2 = 0; x2 < mx2; x2 += pot)
						{
							int x234 = x2 + x34;
							int y234 = y2 + y34;
							int my1 = std::min(pot, h - y234);
							int mx1 = std::min(pot, w - x234);

							int best_idx_2 = -1;
							float best_val_2 = 0;

							Eigen::Matrix<float, 2, 1> dir2 = directions[random_pattern[n2] & 0xF];

							for(int y1 = 0; y1 < my1; y1 += 1)
							{
								for(int x1 = 0; x1 < mx1; x1 += 1)
								{
									assert(x1 + x234 < w);
									assert(y1 + y234 < h);

									int idx = x1 + x234 + w * (y1 + y234);
									int xf = x1 + x234;
									int yf = y1 + y234;

									if(xf < 4 || xf >= w - 5 || yf < 4 || yf > h - 4) continue;

									if (fh->mask_left[idx] == true) continue;

									float pixel_TH_0 = ths_smoothed[(xf >> 5) + (yf >> 5) * ths_step];
									float pixel_TH_1 = pixel_TH_0 * dw1;
									float pixel_TH_2 = pixel_TH_1 * dw2;

									float ag_0 = mapmax_0[idx];

									if(ag_0 > pixel_TH_0 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir2)));

										if(!setting_select_direction_distribution) dir_norm = ag_0;

										if(dir_norm > best_val_2)
										{
											best_val_2 = dir_norm;
											best_idx_2 = idx;
											best_idx_3 = -2;
											best_idx_4 = -2;
										}
									}

									if(best_idx_3 == -2) continue;

									float ag_1 = mapmax_1[(int)(xf * 0.5f + 0.25f) + (int)(yf * 0.5f + 0.25f) * w1];

									if(ag_1 > pixel_TH_1 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir3)));

										if(!setting_select_direction_distribution) dir_norm = ag_1;

										if(dir_norm > best_val_3)
										{
											best_val_3 = dir_norm;
											best_idx_3 = idx;
											best_idx_4 = -2;
										}
									}

									if(best_idx_4 == -2) continue;

									float ag_2 = mapmax_2[(int)(xf * 0.25f + 0.125) + (int)(yf * 0.25f + 0.125) * w2];

									if(ag_2 > pixel_TH_2 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir4)));

										if(!setting_select_direction_distribution) dir_norm = ag_2;

										if(dir_norm > best_val_4)
										{
											best_val_4 = dir_norm;
											best_idx_4 = idx;
										}
									}
								}
							}

							if(best_idx_2 > 0)
							{
								fh->selected_pixels_left[best_idx_2] = 1;
								best_val_3 = 1e10;
								n2++;
							}
						}
					}

					if(best_idx_3 > 0)
					{
						fh->selected_pixels_left[best_idx_3] = 2;
						best_val_4 = 1e10;
						n3++;
					}
				}

				if(best_idx_4 > 0)
				{
					fh->selected_pixels_left[best_idx_4] = 4;
					n4++;
				}
			}
		}
	}

	return Eigen::Vector3i(n2, n3, n4);
}
    Eigen::Vector3i selectRight(std::shared_ptr<frame> fh, int pot, float th_factor = 1) {
	Eigen::Vector3f const * const map_0 = fh->dI_right;

	float* mapmax_0 = fh->abs_squared_grad_right[0];
	float* mapmax_1 = fh->abs_squared_grad_right[1];
	float* mapmax_2 = fh->abs_squared_grad_right[2];

	int w = wG[0];
	int w1 = wG[1];
	int w2 = wG[2];
	int h = hG[0];

	const Eigen::Matrix<float, 2, 1> directions[16] = {
		Eigen::Matrix<float, 2, 1>(0, 1.0000),
		Eigen::Matrix<float, 2, 1>(0.3827, 0.9239),
		Eigen::Matrix<float, 2, 1>(0.1951, 0.9808),
		Eigen::Matrix<float, 2, 1>(0.9239, 0.3827),
		Eigen::Matrix<float, 2, 1>(0.7071, 0.7071),
		Eigen::Matrix<float, 2, 1>(0.3827, -0.9239),
		Eigen::Matrix<float, 2, 1>(0.8315, 0.5556),
		Eigen::Matrix<float, 2, 1>(0.8315, -0.5556),
		Eigen::Matrix<float, 2, 1>(0.5556, -0.8315),
		Eigen::Matrix<float, 2, 1>(0.9808, 0.1951),
		Eigen::Matrix<float, 2, 1>(0.9239, -0.3827),
		Eigen::Matrix<float, 2, 1>(0.7071, -0.7071),
		Eigen::Matrix<float, 2, 1>(0.5556, 0.8315),
		Eigen::Matrix<float, 2, 1>(0.9808, -0.1951),
		Eigen::Matrix<float, 2, 1>(1.0000, 0.0000),
		Eigen::Matrix<float, 2, 1>(0.1951, -0.9808)};

	memset(fh->selected_pixels_right, 0, w * h * sizeof(PixelSelectorStatus));

	float dw1 = setting_grad_down_weight_per_level;
	float dw2 = dw1 * dw1;

	int n3 = 0, n2 = 0, n4 = 0;

	for(int y4 = 0; y4 < h; y4 += (4 * pot))
	{
		for(int x4 = 0; x4 < w; x4 += (4 * pot))
		{
			int my3 = std::min((4 * pot), h - y4);
			int mx3 = std::min((4 * pot), w - x4);
			int best_idx_4 = -1;
			float best_val_4 = 0;

			Eigen::Matrix<float, 2, 1> dir4 = directions[random_pattern[n2] & 0xF];

			for(int y3 = 0; y3 < my3; y3 += (2 * pot))
			{
				for(int x3 = 0; x3 < mx3; x3 += (2 * pot))
				{
					int x34 = x3 + x4;
					int y34 = y3 + y4;
					int my2 = std::min((2 * pot), h - y34);
					int mx2 = std::min((2 * pot), w - x34);

					int best_idx_3 = -1;
					float best_val_3 = 0;

					Eigen::Matrix<float, 2, 1> dir3 = directions[random_pattern[n2] & 0xF];

					for(int y2 = 0; y2 < my2; y2 += pot)
					{
						for(int x2 = 0; x2 < mx2; x2 += pot)
						{
							int x234 = x2 + x34;
							int y234 = y2 + y34;
							int my1 = std::min(pot, h - y234);
							int mx1 = std::min(pot, w - x234);

							int best_idx_2 = -1;
							float best_val_2 = 0;

							Eigen::Matrix<float, 2, 1> dir2 = directions[random_pattern[n2] & 0xF];

							for(int y1 = 0; y1 < my1; y1 += 1)
							{
								for(int x1 = 0; x1 < mx1; x1 += 1)
								{
									assert(x1 + x234 < w);
									assert(y1 + y234 < h);

									int idx = x1 + x234 + w * (y1 + y234);
									int xf = x1 + x234;
									int yf = y1 + y234;

									if (xf < 4 || xf >= w - 5 || yf < 4 || yf > h - 4) continue;

									if (fh->mask_right[idx] == true) continue;

									float pixel_TH_0 = ths_smoothed[(xf >> 5) + (yf >> 5) * ths_step];
									float pixel_TH_1 = pixel_TH_0 * dw1;
									float pixel_TH_2 = pixel_TH_1 * dw2;

									float ag_0 = mapmax_0[idx];

									if(ag_0 > pixel_TH_0 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir2)));

										if(!setting_select_direction_distribution) dir_norm = ag_0;

										if(dir_norm > best_val_2)
										{
											best_val_2 = dir_norm;
											best_idx_2 = idx;
											best_idx_3 = -2;
											best_idx_4 = -2;
										}
									}

									if(best_idx_3 == -2) continue;

									float ag_1 = mapmax_1[(int)(xf * 0.5f + 0.25f) + (int)(yf * 0.5f + 0.25f) * w1];

									if(ag_1 > pixel_TH_1 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir3)));

										if(!setting_select_direction_distribution) dir_norm = ag_1;

										if(dir_norm > best_val_3)
										{
											best_val_3 = dir_norm;
											best_idx_3 = idx;
											best_idx_4 = -2;
										}
									}

									if(best_idx_4 == -2) continue;

									float ag_2 = mapmax_2[(int)(xf * 0.25f + 0.125) + (int)(yf * 0.25f + 0.125) * w2];

									if(ag_2 > pixel_TH_2 * th_factor)
									{
										Eigen::Matrix<float, 2, 1> ag_0_d = map_0[idx].tail<2>();
										float dir_norm = fabsf((float)(ag_0_d.dot(dir4)));

										if(!setting_select_direction_distribution) dir_norm = ag_2;

										if(dir_norm > best_val_4)
										{
											best_val_4 = dir_norm;
											best_idx_4 = idx;
										}
									}
								}
							}

							if(best_idx_2 > 0)
							{
								fh->selected_pixels_right[best_idx_2] = 1;
								best_val_3 = 1e10;
								n2++;
							}
						}
					}

					if(best_idx_3 > 0)
					{
						fh->selected_pixels_right[best_idx_3] = 2;
						best_val_4 = 1e10;
						n3++;
					}
				}

				if(best_idx_4 > 0)
				{
					fh->selected_pixels_right[best_idx_4] = 4;
					n4++;
				}
			}
		}
	}

	return Eigen::Vector3i(n2, n3, n4);
}
    int current_potential;
    bool allow_fast;
    unsigned char* random_pattern;
    int* grad_hist;
    float* ths;
    float* ths_smoothed;
    int ths_step;
    std::shared_ptr<frame> grad_hist_frame;
};

pixelSelector(int w, int h) {
	random_pattern = new unsigned char[w * h];
	std::srand(3141592);
	for (int i = 0; i < w * h; i++)
		random_pattern[i] = rand() & 0xFF;
	current_potential = 3;
	grad_hist = new int[100 * (1 + w / 32) * (1 + h / 32)];
	ths = new float[(w / 32) * (h / 32) + 100];
	ths_smoothed = new float[(w / 32) * (h / 32) + 100];
	allow_fast = false;
	grad_hist_frame = nullptr;
}

class feature {
  public:
    trackKLT(std::unordered_map<size_t, std::shared_ptr<cameraBase>> camera_calib_, int num_features_,
		HistogramMethod histogram_method_, int fast_threshold_, int patch_size_x_, int patch_size_y_, int min_px_dist_) {
  current_id = 1;

  pixelSelector_ptr = std::make_shared<pixelSelector>(wG[0], hG[0]);
}
    void feedNewImage(const cameraData &image_measurements, std::shared_ptr<frame> fh) {
  if (image_measurements.camera_ids.empty() || image_measurements.camera_ids.size() != image_measurements.images.size()
    || image_measurements.images.size() != image_measurements.masks.size())
  {
    std::cout << "[ERROR]: image_measurements data sizes do not match or empty." << std::endl;
    std::cout << "[ERROR]: - image_measurements.camera_ids.size() = " << image_measurements.camera_ids.size() << std::endl;
    std::cout << "[ERROR]: - image_measurements.images.size() = " << image_measurements.images.size() << std::endl;
    std::cout << "[ERROR]: - image_measurements.masks.size() = " << image_measurements.masks.size() << std::endl;
    std::exit(EXIT_FAILURE);
  }

  rT1 = boost::posix_time::microsec_clock::local_time();
  size_t num_images = image_measurements.images.size();

  for (size_t image_id = 0; image_id < num_images; image_id++)
  {
    size_t cam_id = image_measurements.camera_ids.at(image_id);

    cv::Mat image;

    if (histogram_method == HistogramMethod::HISTOGRAM)
    {
      cv::equalizeHist(image_measurements.images.at(image_id), image);
    }
    else if (histogram_method == HistogramMethod::CLAHE)
    {
      double eq_clip_limit = 10.0;
      cv::Size eq_win_size = cv::Size(8, 8);
      cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
      clahe->apply(image_measurements.images.at(image_id), image);
    }
    else
    {
      image = image_measurements.images.at(image_id);
    }

    std::vector<cv::Mat> img_pyr;
    cv::buildOpticalFlowPyramid(image, img_pyr, win_size, pyr_levels);

    img_curr[cam_id] = image;
    img_pyramid_curr[cam_id] = img_pyr;
  }

  feedStereo(image_measurements, fh, 0, 1);
}
    std::shared_ptr<featureDatabase> getFeatureDatabase() {
  return database;
}
    std::unordered_map<size_t, std::vector<cv::KeyPoint>> getLastObs() {
  return pts_last;
}
    std::unordered_map<size_t, std::vector<size_t>> getLastIds() {
  return ids_last;
}
    void setNumFeatures(int num_features_) {
  num_features = num_features_;
}
    void displayActive(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2, std::string overlay = "") {
  std::map<size_t, cv::Mat> img_last_cache, img_mask_last_cache;
  std::unordered_map<size_t, std::vector<cv::KeyPoint>> pts_last_cache;
  {
    img_last_cache = img_last;
    img_mask_last_cache = img_mask_last;
    pts_last_cache = pts_last;
  }

  int max_width = -1;
  int max_height = -1;

  for (auto const &pair : img_last_cache)
  {
    if (max_width < pair.second.cols)
      max_width = pair.second.cols;
    if (max_height < pair.second.rows)
      max_height = pair.second.rows;
  }

  if (img_last_cache.empty() || max_width == -1 || max_height == -1)
    return;

  bool is_small = (std::min(max_width, max_height) < 400);

  bool image_new = ((int)img_last_cache.size() * max_width != img_out.cols || max_height != img_out.rows);

  if (image_new)
    img_out = cv::Mat(max_height, (int)img_last_cache.size() * max_width, CV_8UC3, cv::Scalar(0, 0, 0));

  int index_cam = 0;
  for (auto const &pair : img_last_cache)
  {
    cv::Mat img_temp;

    if (image_new)
      cv::cvtColor(img_last_cache[pair.first], img_temp, cv::COLOR_GRAY2RGB);
    else
      img_temp = img_out(cv::Rect(max_width * index_cam, 0, max_width, max_height));

    for (size_t i = 0; i < pts_last_cache[pair.first].size(); i++)
    {
      cv::Point2f pt_l = pts_last_cache[pair.first].at(i).pt;
      cv::circle(img_temp, pt_l, (is_small) ? 1 : 2, cv::Scalar(r1, g1, b1), cv::FILLED);

      cv::Point2f pt_l_top = cv::Point2f(pt_l.x - 3, pt_l.y - 3);
      cv::Point2f pt_l_bot = cv::Point2f(pt_l.x + 3, pt_l.y + 3);
      cv::rectangle(img_temp, pt_l_top, pt_l_bot, cv::Scalar(r2, g2, b2), 1);
    }

    auto txtpt = (is_small) ? cv::Point(10, 30) : cv::Point(30, 60);

    if (overlay == "")
    {
      cv::putText(img_temp, "CAM:" + std::to_string((int)pair.first), txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small) ? 1.5 : 3.0, cv::Scalar(0, 255, 0), 3);
    }
    else
    {
      cv::putText(img_temp, overlay, txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small) ? 1.5 : 3.0, cv::Scalar(0, 0, 255), 3);
    }

    cv::Mat mask = cv::Mat::zeros(img_mask_last_cache[pair.first].rows, img_mask_last_cache[pair.first].cols, CV_8UC3);
    mask.setTo(cv::Scalar(0, 0, 255), img_mask_last_cache[pair.first]);
    cv::addWeighted(mask, 0.1, img_temp, 1.0, 0.0, img_temp);

    img_temp.copyTo(img_out(cv::Rect(max_width * index_cam, 0, img_last_cache[pair.first].cols, img_last_cache[pair.first].rows)));
    index_cam++;
  }
}
  private:
    void feedStereo(const cameraData &image_measurements, std::shared_ptr<frame> fh, size_t image_id_left, size_t image_id_right) {
  size_t cam_id_left = image_measurements.camera_ids.at(image_id_left);
  size_t cam_id_right = image_measurements.camera_ids.at(image_id_right);

  cv::Mat img_left = img_curr.at(cam_id_left);
  cv::Mat img_right = img_curr.at(cam_id_right);
  std::vector<cv::Mat> img_pyr_left = img_pyramid_curr.at(cam_id_left);
  std::vector<cv::Mat> img_pyr_right = img_pyramid_curr.at(cam_id_right);
  cv::Mat mask_left = image_measurements.masks.at(image_id_left);
  cv::Mat mask_right = image_measurements.masks.at(image_id_right);
  rT2 = boost::posix_time::microsec_clock::local_time();

  if (pts_last[cam_id_left].empty() && pts_last[cam_id_right].empty())
  {
    std::vector<cv::KeyPoint> good_left, good_right;
    std::vector<size_t> good_ids_left, good_ids_right;
    performDetectionStereo(fh, img_pyr_left, img_pyr_right, mask_left, mask_right, cam_id_left, cam_id_right, good_left, good_right, good_ids_left, good_ids_right);

    img_last[cam_id_left] = img_left;
    img_last[cam_id_right] = img_right;
    img_pyramid_last[cam_id_left] = img_pyr_left;
    img_pyramid_last[cam_id_right] = img_pyr_right;
    img_mask_last[cam_id_left] = mask_left;
    img_mask_last[cam_id_right] = mask_right;
    pts_last[cam_id_left] = good_left;
    pts_last[cam_id_right] = good_right;
    ids_last[cam_id_left] = good_ids_left;
    ids_last[cam_id_right] = good_ids_right;

    return;
  }

  int pts_before_detect = (int)pts_last[cam_id_left].size();
  auto pts_left_old = pts_last[cam_id_left];
  auto pts_right_old = pts_last[cam_id_right];
  auto ids_left_old = ids_last[cam_id_left];
  auto ids_right_old = ids_last[cam_id_right];
  performDetectionStereo(fh, img_pyramid_last[cam_id_left], img_pyramid_last[cam_id_right], img_mask_last[cam_id_left],
    img_mask_last[cam_id_right], cam_id_left, cam_id_right, pts_left_old, pts_right_old, ids_left_old, ids_right_old);

  rT3 = boost::posix_time::microsec_clock::local_time();

  std::vector<uchar> mask_ll, mask_rr;
  std::vector<cv::KeyPoint> pts_left_new = pts_left_old;
  std::vector<cv::KeyPoint> pts_right_new = pts_right_old;

  parallel_for_(cv::Range(0, 2), lambdaBody([&](const cv::Range &range) {
                  for (int i = range.start; i < range.end; i++) {
                    bool is_left = (i == 0);
                    performMatching(img_pyramid_last[is_left ? cam_id_left : cam_id_right], is_left ? img_pyr_left : img_pyr_right,
                                    is_left ? pts_left_old : pts_right_old, is_left ? pts_left_new : pts_right_new,
                                    is_left ? cam_id_left : cam_id_right, is_left ? cam_id_left : cam_id_right,
                                    is_left ? mask_ll : mask_rr);
                  }
                }));
  rT4 = boost::posix_time::microsec_clock::local_time();

  rT5 = boost::posix_time::microsec_clock::local_time();

  if (mask_ll.empty() && mask_rr.empty())
  {
    img_last[cam_id_left] = img_left;
    img_last[cam_id_right] = img_right;
    img_pyramid_last[cam_id_left] = img_pyr_left;
    img_pyramid_last[cam_id_right] = img_pyr_right;
    img_mask_last[cam_id_left] = mask_left;
    img_mask_last[cam_id_right] = mask_right;
    pts_last[cam_id_left].clear();
    pts_last[cam_id_right].clear();
    ids_last[cam_id_left].clear();
    ids_last[cam_id_right].clear();
    std::cout << "[trackKLT::feedStereo]: Not enough points for RANSAC, resetting." << std::endl;

    return;
  }

  std::vector<cv::KeyPoint> good_left, good_right;
  std::vector<size_t> good_ids_left, good_ids_right;

  for (size_t i = 0; i < pts_left_new.size(); i++)
  {
    if (pts_left_new.at(i).pt.x < 0 || pts_left_new.at(i).pt.y < 0 || (int)pts_left_new.at(i).pt.x > img_left.cols ||
      (int)pts_left_new.at(i).pt.y > img_left.rows)
      continue;

    bool found_right = false;
    size_t index_right = 0;
    for (size_t n = 0; n < ids_right_old.size(); n++)
    {
      if (ids_left_old.at(i) == ids_right_old.at(n))
      {
        found_right = true;
        index_right = n;
        break;
      }
    }

    if (mask_ll[i] && found_right && mask_rr[index_right])
    {
      if (pts_right_new.at(index_right).pt.x < 0 || pts_right_new.at(index_right).pt.y < 0 ||
        (int)pts_right_new.at(index_right).pt.x >= img_right.cols || (int)pts_right_new.at(index_right).pt.y >= img_right.rows)
        continue;

      good_left.push_back(pts_left_new.at(i));
      good_right.push_back(pts_right_new.at(index_right));
      good_ids_left.push_back(ids_left_old.at(i));
      good_ids_right.push_back(ids_right_old.at(index_right));
    }
    else if (mask_ll[i])
    {
      good_left.push_back(pts_left_new.at(i));
      good_ids_left.push_back(ids_left_old.at(i));
    }
  }

  for (size_t i = 0; i < pts_right_new.size(); i++)
  {
    if (pts_right_new.at(i).pt.x < 0 || pts_right_new.at(i).pt.y < 0 || (int)pts_right_new.at(i).pt.x >= img_right.cols ||
      (int)pts_right_new.at(i).pt.y >= img_right.rows)
      continue;

    bool added_already = (std::find(good_ids_right.begin(), good_ids_right.end(), ids_right_old.at(i)) != good_ids_right.end());

    if (mask_rr[i] && !added_already)
    {
      good_right.push_back(pts_right_new.at(i));
      good_ids_right.push_back(ids_right_old.at(i));
    }
  }

  for (size_t i = 0; i < good_left.size(); i++)
  {
    cv::Point2f npt_l = camera_calib.at(cam_id_left)->undistortCV(good_left.at(i).pt);
    database->updateFeature(fh, good_ids_left.at(i), image_measurements.timestamp, cam_id_left, good_left.at(i).pt.x, good_left.at(i).pt.y, npt_l.x, npt_l.y);
  }
  for (size_t i = 0; i < good_right.size(); i++)
  {
    cv::Point2f npt_r = camera_calib.at(cam_id_right)->undistortCV(good_right.at(i).pt);
    database->updateFeature(fh, good_ids_right.at(i), image_measurements.timestamp, cam_id_right, good_right.at(i).pt.x, good_right.at(i).pt.y, npt_r.x, npt_r.y);
  }

  {
    img_last[cam_id_left] = img_left;
    img_last[cam_id_right] = img_right;
    img_pyramid_last[cam_id_left] = img_pyr_left;
    img_pyramid_last[cam_id_right] = img_pyr_right;
    img_mask_last[cam_id_left] = mask_left;
    img_mask_last[cam_id_right] = mask_right;
    pts_last[cam_id_left] = good_left;
    pts_last[cam_id_right] = good_right;
    ids_last[cam_id_left] = good_ids_left;
    ids_last[cam_id_right] = good_ids_right;
  }
  rT6 = boost::posix_time::microsec_clock::local_time();

  // Time test
  /*
  std::cout << std::fixed << "[trackKLT]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " seconds for pyramid." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " seconds for detection (" << (int)pts_last[cam_id_left].size() - pts_before_detect << " detected)." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT4 - rT3).total_microseconds() * 1e-6 << " seconds for temporal klt." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT5 - rT4).total_microseconds() * 1e-6 << " seconds for stereo klt." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT6 - rT5).total_microseconds() * 1e-6 << " seconds for feature DB update (" << (int)good_left.size() << " features)." << std::endl;
  std::cout << std::fixed << "[trackKLT]: " << (rT6 - rT1).total_microseconds() * 1e-6 << " seconds for total." << std::endl;
  */
  // Time test
}
    void performDetectionStereo(std::shared_ptr<frame> fh, const std::vector<cv::Mat> &img_0_pyr, const std::vector<cv::Mat> &img_1_pyr, const cv::Mat &mask_0, const cv::Mat &mask_1,
		size_t cam_id_left, size_t cam_id_right, std::vector<cv::KeyPoint> &pts_0, std::vector<cv::KeyPoint> &pts_1, std::vector<size_t> &ids_0, std::vector<size_t> &ids_1) {
  cv::Size size_close_0((int)((float)img_0_pyr.at(0).cols / (float)min_px_dist),
                        (int)((float)img_0_pyr.at(0).rows / (float)min_px_dist));
  cv::Mat grid_2d_close_0 = cv::Mat::zeros(size_close_0, CV_8UC1);
  float size_x_0 = (float)img_0_pyr.at(0).cols / (float)patch_size_x;
  float size_y_0 = (float)img_0_pyr.at(0).rows / (float)patch_size_y;

  cv::Size size_grid_0(patch_size_x, patch_size_y);
  cv::Mat grid_2d_grid_0 = cv::Mat::zeros(size_grid_0, CV_8UC1);
  cv::Mat mask_0_updated = mask_0.clone();

  auto it_0 = pts_0.begin();
  auto it_1 = ids_0.begin();

  while (it_0 != pts_0.end())
  {
    cv::KeyPoint kpt = *it_0;
    int x = (int)kpt.pt.x;
    int y = (int)kpt.pt.y;

    int edge = 10;
    if (x < edge || x >= img_0_pyr.at(0).cols - edge || y < edge || y >= img_0_pyr.at(0).rows - edge)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    int x_close = (int)(kpt.pt.x / (float)min_px_dist);
    int y_close = (int)(kpt.pt.y / (float)min_px_dist);
    if (x_close < 0 || x_close >= size_close_0.width || y_close < 0 || y_close >= size_close_0.height)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    int x_grid = std::floor(kpt.pt.x / size_x_0);
    int y_grid = std::floor(kpt.pt.y / size_y_0);

    if (x_grid < 0 || x_grid >= size_grid_0.width || y_grid < 0 || y_grid >= size_grid_0.height)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    if (grid_2d_close_0.at<uint8_t>(y_close, x_close) > 127)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    if (mask_0.at<uint8_t>(y, x) > 127)
    {
      it_0 = pts_0.erase(it_0);
      it_1 = ids_0.erase(it_1);
      continue;
    }

    grid_2d_close_0.at<uint8_t>(y_close, x_close) = 255;
    if (grid_2d_grid_0.at<uint8_t>(y_grid, x_grid) < 255)
    {
      grid_2d_grid_0.at<uint8_t>(y_grid, x_grid) += 1;
    }

    if (x - min_px_dist >= 0 && x + min_px_dist < img_0_pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img_0_pyr.at(0).rows)
    {
      cv::Point pt_1(x - min_px_dist, y - min_px_dist);
      cv::Point pt_2(x + min_px_dist, y + min_px_dist);
      cv::rectangle(mask_0_updated, pt_1, pt_2, cv::Scalar(255), -1);

      int begin_idx = x - min_px_dist + wG[0] * (y - min_px_dist);
      int end_idx = x + min_px_dist + wG[0] * (y + min_px_dist);

      for(int i = begin_idx; i <= end_idx; i++) fh->mask_left[i] = true;
    }

    it_0++;
    it_1++;
  }

  double min_feat_percent = 0.50;
  int num_featsneeded_0 = num_features - (int)pts_0.size();

  if (num_featsneeded_0 > std::min(20, (int)(min_feat_percent * num_features)))
  {
    pixelSelector_ptr->pixelSelectionLeft(fh, num_featsneeded_0);

    std::vector<cv::KeyPoint> kpts_0_new;
    std::vector<cv::Point2f> pts_0_new;

    for (int y = 0; y < hG[0]; y++)
    {
      for (int x = 0; x < wG[0]; x++)
      {
        int idx = x + y * wG[0];

        if (fh->selected_pixels_left[idx] == 0) continue;

        cv::KeyPoint kpt;
        kpt.pt.x = x;
        kpt.pt.y = y;

        kpts_0_new.push_back(kpt);
        pts_0_new.push_back(kpt.pt);
      }
    }

    std::vector<cv::KeyPoint> kpts_1_new;
    std::vector<cv::Point2f> pts_1_new;
    kpts_1_new = kpts_0_new;
    pts_1_new = pts_0_new;

    if (!pts_0_new.empty())
    {
      std::vector<uchar> mask;
      std::vector<float> error;
      cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01);
      cv::calcOpticalFlowPyrLK(img_0_pyr, img_1_pyr, pts_0_new, pts_1_new, mask, error, win_size, pyr_levels, term_crit, cv::OPTFLOW_USE_INITIAL_FLOW);

      for (size_t i = 0; i < pts_0_new.size(); i++)
      {
        bool oob_left = ((int)pts_0_new.at(i).x < 0 || (int)pts_0_new.at(i).x >= img_0_pyr.at(0).cols || (int)pts_0_new.at(i).y < 0 ||
                         (int)pts_0_new.at(i).y >= img_0_pyr.at(0).rows);
        bool oob_right = ((int)pts_1_new.at(i).x < 0 || (int)pts_1_new.at(i).x >= img_1_pyr.at(0).cols || (int)pts_1_new.at(i).y < 0 ||
                          (int)pts_1_new.at(i).y >= img_1_pyr.at(0).rows);

        if (!oob_left && !oob_right && mask[i] == 1)
        {
          kpts_0_new.at(i).pt = pts_0_new.at(i);
          kpts_1_new.at(i).pt = pts_1_new.at(i);

          pts_0.push_back(kpts_0_new.at(i));
          pts_1.push_back(kpts_1_new.at(i));

          size_t temp = ++current_id;
          ids_0.push_back(temp);
          ids_1.push_back(temp);
        }
        else if (!oob_left)
        {
          kpts_0_new.at(i).pt = pts_0_new.at(i);
          pts_0.push_back(kpts_0_new.at(i));

          size_t temp = ++current_id;
          ids_0.push_back(temp);
        }
      }
    }
  }

  cv::Size size_close_1((int)((float)img_1_pyr.at(0).cols / (float)min_px_dist), (int)((float)img_1_pyr.at(0).rows / (float)min_px_dist));
  cv::Mat grid_2d_close_1 = cv::Mat::zeros(size_close_1, CV_8UC1);
  float size_x_1 = (float)img_1_pyr.at(0).cols / (float)patch_size_x;
  float size_y_1 = (float)img_1_pyr.at(0).rows / (float)patch_size_y;

  cv::Size size_grid_1(patch_size_x, patch_size_y);
  cv::Mat grid_2d_grid_1 = cv::Mat::zeros(size_grid_1, CV_8UC1);
  cv::Mat mask_1_updated = mask_0.clone();

  it_0 = pts_1.begin();
  it_1 = ids_1.begin();

  while (it_0 != pts_1.end())
  {
    cv::KeyPoint kpt = *it_0;
    int x = (int)kpt.pt.x;
    int y = (int)kpt.pt.y;
    int edge = 10;
    if (x < edge || x >= img_1_pyr.at(0).cols - edge || y < edge || y >= img_1_pyr.at(0).rows - edge)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    int x_close = (int)(kpt.pt.x / (float)min_px_dist);
    int y_close = (int)(kpt.pt.y / (float)min_px_dist);
    if (x_close < 0 || x_close >= size_close_1.width || y_close < 0 || y_close >= size_close_1.height)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    int x_grid = std::floor(kpt.pt.x / size_x_1);
    int y_grid = std::floor(kpt.pt.y / size_y_1);
    if (x_grid < 0 || x_grid >= size_grid_1.width || y_grid < 0 || y_grid >= size_grid_1.height)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    bool is_stereo = (std::find(ids_0.begin(), ids_0.end(), *it_1) != ids_0.end());

    if (grid_2d_close_1.at<uint8_t>(y_close, x_close) > 127 && !is_stereo)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    if (mask_1.at<uint8_t>(y, x) > 127)
    {
      it_0 = pts_1.erase(it_0);
      it_1 = ids_1.erase(it_1);
      continue;
    }

    grid_2d_close_1.at<uint8_t>(y_close, x_close) = 255;

    if (grid_2d_grid_1.at<uint8_t>(y_grid, x_grid) < 255)
    {
      grid_2d_grid_1.at<uint8_t>(y_grid, x_grid) += 1;
    }

    if (x - min_px_dist >= 0 && x + min_px_dist < img_1_pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img_1_pyr.at(0).rows)
    {
      cv::Point pt_1(x - min_px_dist, y - min_px_dist);
      cv::Point pt_2(x + min_px_dist, y + min_px_dist);
      cv::rectangle(mask_1_updated, pt_1, pt_2, cv::Scalar(255), -1);

      int begin_idx = x - min_px_dist + wG[0] * (y - min_px_dist);
      int end_idx = x + min_px_dist + wG[0] * (y + min_px_dist);

      for(int i = begin_idx; i <= end_idx; i++) fh->mask_right[i] = true;
    }

    it_0++;
    it_1++;
  }

  int num_featsneeded_1 = num_features - (int)pts_1.size();
  if (num_featsneeded_1 > std::min(20, (int)(min_feat_percent * num_features)))
  {
    pixelSelector_ptr->pixelSelectionRight(fh, num_featsneeded_1);

    for (int y = 0; y < hG[0]; y++)
    {
      for (int x = 0; x < wG[0]; x++)
      {
        int idx = x + y * wG[0];

        if (fh->selected_pixels_right[idx] == 0) continue;

        cv::KeyPoint kpt;
        kpt.pt.x = x;
        kpt.pt.y = y;

        pts_1.push_back(kpt);
        size_t temp = ++current_id;
        ids_1.push_back(temp);
      }
    }
  }
}
    void performMatching(const std::vector<cv::Mat> &img_0_pyr, const std::vector<cv::Mat> &img_1_pyr, std::vector<cv::KeyPoint> &pts_0,
		std::vector<cv::KeyPoint> &pts_1, size_t id_0, size_t id_1, std::vector<uchar> &mask_out) {
  assert(kpts_0.size() == kpts_1.size());

  if (kpts_0.empty() || kpts_1.empty())
    return;

  std::vector<cv::Point2f> pts_0, pts_1;
  for (size_t i = 0; i < kpts_0.size(); i++)
  {
    pts_0.push_back(kpts_0.at(i).pt);
    pts_1.push_back(kpts_1.at(i).pt);
  }

  if (pts_0.size() < 10)
  {
    for (size_t i = 0; i < pts_0.size(); i++)
      mask_out.push_back((uchar)0);

    return;
  }

  std::vector<uchar> mask_klt;
  std::vector<float> error;
  cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01);
  cv::calcOpticalFlowPyrLK(img_0_pyr, img_1_pyr, pts_0, pts_1, mask_klt, error, win_size, pyr_levels, term_crit, cv::OPTFLOW_USE_INITIAL_FLOW);

  std::vector<cv::Point2f> pts_0_n, pts_1_n;
  for (size_t i = 0; i < pts_0.size(); i++)
  {
    pts_0_n.push_back(camera_calib.at(id_0)->undistortCV(pts_0.at(i)));
    pts_1_n.push_back(camera_calib.at(id_1)->undistortCV(pts_1.at(i)));
  }

  std::vector<uchar> mask_rsc;
  double max_focallength_img_0 = std::max(camera_calib.at(id_0)->getK()(0, 0), camera_calib.at(id_0)->getK()(1, 1));
  double max_focallength_img_1 = std::max(camera_calib.at(id_1)->getK()(0, 0), camera_calib.at(id_1)->getK()(1, 1));
  double max_focallength = std::max(max_focallength_img_0, max_focallength_img_1);
  cv::findFundamentalMat(pts_0_n, pts_1_n, cv::FM_RANSAC, 2.0 / max_focallength, 0.999, mask_rsc);

  for (size_t i = 0; i < mask_klt.size(); i++)
  {
    auto mask = (uchar)((i < mask_klt.size() && mask_klt[i] && i < mask_rsc.size() && mask_rsc[i]) ? 1 : 0);
    mask_out.push_back(mask);
  }

  for (size_t i = 0; i < pts_0.size(); i++)
  {
    kpts_0.at(i).pt = pts_0.at(i);
    kpts_1.at(i).pt = pts_1.at(i);
  }
}
    std::unordered_map<size_t, std::shared_ptr<cameraBase>> camera_calib;
    std::shared_ptr<featureDatabase> database;
    int num_features;
    HistogramMethod histogram_method;
    std::map<size_t, cv::Mat> img_last;
    std::map<size_t, cv::Mat> img_mask_last;
    std::unordered_map<size_t, std::vector<cv::KeyPoint>> pts_last;
    std::unordered_map<size_t, std::vector<size_t>> ids_last;
    std::atomic<size_t> current_id;
    std::shared_ptr<pixelSelector> pixelSelector_ptr;
    int threshold;
    int patch_size_x;
    int patch_size_y;
    int min_px_dist;
    int pyr_levels = 5;
    cv::Size win_size = cv::Size(15, 15);
    std::map<size_t, std::vector<cv::Mat>> img_pyramid_last;
    std::map<size_t, cv::Mat> img_curr;
    std::map<size_t, std::vector<cv::Mat>> img_pyramid_curr;
    boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7;
};

class staticInitializer {
  public:
    inertialInitializer(inertialInitializerOptions &initializer_options_, std::shared_ptr<featureDatabase> db_) {
	imu_data = std::make_shared<std::vector<imuData>>();

	static_initializer = std::make_shared<staticInitializer>(initializer_options, db, imu_data);
	dynamic_initializer = std::make_shared<dynamicInitializer>(initializer_options, db, imu_data);
}
    void feedImu(const imuData &imu_data_, double oldest_time = -1) {
	imu_data->emplace_back(imu_data_);

	if (oldest_time != -1)
	{
		auto it0 = imu_data->begin();

		while (it0 != imu_data->end())
		{
			if (it0->timestamp < oldest_time)
				it0 = imu_data->erase(it0);
			else
				it0++;
		}
	}
}
    bool initialize(double &timestamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<baseType>> &order,
    std::shared_ptr<imuState> imu_state_, bool wait_for_jerk = true) {
	auto rT1 = boost::posix_time::microsec_clock::local_time();
	double newest_cam_time = -1;
	for (auto const &feat : db->getInternalData())
	{
		for (auto const &cam_time_pair : feat.second->timestamps)
		{
			for (auto const &time : cam_time_pair.second)
			{
				newest_cam_time = std::max(newest_cam_time, time);
			}
		}
	}

	double oldest_time = newest_cam_time - initializer_options.init_window_time;

	if (newest_cam_time < 0 || oldest_time < 0) return false;

	db->cleanUpOldmeasurements(oldest_time);

	bool have_old_imu_readings = false;
	auto it_imu = imu_data->begin();

	while (it_imu != imu_data->end() && it_imu->timestamp < oldest_time + initializer_options.calib_camimu_dt)
	{
		have_old_imu_readings = true;
		it_imu = imu_data->erase(it_imu);
	}

	if (db->getInternalData().size() < 0.75 * initializer_options.init_max_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Only " << db->getInternalData().size() << " valid features of required ("
			<< 0.95 * initializer_options.init_max_features << " thresh)" << std::endl;
		return false;
	}

	if (imu_data->size() < 2 || !have_old_imu_readings)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Waiting for window to reach full size (" << imu_data->size() << " imu readings)" << std::endl;
		return false;
	}

	std::unordered_map<size_t, std::shared_ptr<feature>> features;

	for (const auto &feat : db->getInternalData())
	{
		auto feat_new = std::make_shared<feature>();
		feat_new->feature_id = feat.second->feature_id;
		feat_new->uvs = feat.second->uvs;
		feat_new->uvs_norm = feat.second->uvs_norm;
		feat_new->timestamps = feat.second->timestamps;
		features.insert({feat.first, feat_new});
	}

	const int min_num_meas_to_optimize = (int)initializer_options.init_window_time;
	const int min_valid_features = 8;

	bool have_stereo = false;
	int count_valid_features = 0;
	std::map<size_t, int> map_features_num_meas;
	int num_measurements = 0;
	double oldest_camera_time = INFINITY;
	std::map<double, bool> map_camera_times;
	map_camera_times[newest_cam_time] = true;
	std::map<size_t, bool> map_camera_ids;
	double pose_dt_avg = initializer_options.init_window_time / (double)(initializer_options.init_dyn_num_pose + 1);

	for (auto const &feat : features)
	{
		std::vector<double> times;
		std::map<size_t, bool> cam_ids;

		for (auto const &cam_time : feat.second->timestamps)
		{
			for (double time : cam_time.second)
			{
				double time_dt = INFINITY;

				for (auto const &tmp : map_camera_times)
					time_dt = std::min(time_dt, std::abs(time - tmp.first));

				for (auto const &tmp : times)
					time_dt = std::min(time_dt, std::abs(time - tmp));

				if (time_dt >= pose_dt_avg || time_dt == 0.0)
				{
					times.push_back(time);
					cam_ids[cam_time.first] = true;
				}
			}
		}

		map_features_num_meas[feat.first] = (int)times.size();
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		for (auto const &tmp : times)
		{
			map_camera_times[tmp] = true;
			oldest_camera_time = std::min(oldest_camera_time, tmp);
			num_measurements += 2;
		}

		for (auto const &tmp : cam_ids)
			map_camera_ids[tmp.first] = true;

		if (cam_ids.size() > 1) have_stereo = true;

		count_valid_features++;
	}

	if ((int)map_camera_times.size() < initializer_options.init_dyn_num_pose)
		return false;

	if (count_valid_features < min_valid_features)
	{
		std::cout << "[initialize-dynamic]: Only " << count_valid_features << " valid features of required " << min_valid_features << std::endl;
		return false;
	}

	Eigen::Vector3d gyroscope_bias = initializer_options.init_dyn_bias_g;
	Eigen::Vector3d accelerometer_bias = initializer_options.init_dyn_bias_a;

	double accel_inI_norm = 0.0;
	double theta_inI_norm = 0.0;
	double time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
	double time_1_in_imu = newest_cam_time + initializer_options.calib_camimu_dt;
	std::vector<imuData> readings = initializerHelper::selectImuReadings(*imu_data, time_0_in_imu, time_1_in_imu);

	assert(readings.size() > 2);
	for (size_t k = 0; k < readings.size() - 1; k++)
	{
		auto imu_0 = readings.at(k);
		auto imu_1 = readings.at(k + 1);
		double dt = imu_1.timestamp - imu_0.timestamp;
		Eigen::Vector3d gyr = 0.5 * (imu_0.gyr + imu_1.gyr) - gyroscope_bias;
		Eigen::Vector3d acc = 0.5 * (imu_0.acc + imu_1.acc) - accelerometer_bias;
		theta_inI_norm += ( -gyr * dt).norm();
		accel_inI_norm += acc.norm();
	}

	accel_inI_norm /= (double)(readings.size() - 1);

	if (180.0 / M_PI * theta_inI_norm < initializer_options.init_dyn_min_deg)
	{
		std::cout << "[initialize-dynamic]: Gyroscope only " << 180.0 / M_PI * theta_inI_norm << " degree change ("
			<< initializer_options.init_dyn_min_deg << " thresh)" << std::endl;
		return false;
	}
	std::cout << std::fixed << "[initialize-dynamic]: |theta_I| = " << 180.0 / M_PI * theta_inI_norm
		<< " deg and |accel| = " << accel_inI_norm << std::endl;

 	auto rT2 = boost::posix_time::microsec_clock::local_time();

	const bool use_single_depth = false;
	int size_feature = (use_single_depth) ? 1 : 3;
	int num_features = count_valid_features;
	int system_size = size_feature * num_features + 3 + 3;

	if (num_measurements < system_size)
	{
		std::cout << "[initialize-dynamic]: Not enough feature measurements (" << num_measurements << " meas vs " << system_size
			<< " state size)" << std::endl;

		return false;
	}

	assert(oldest_camera_time < newest_cam_time);
	double last_camera_timestamp = 0.0;
	std::map<double, std::shared_ptr<preIntegrationV1>> map_camera_pre_integration_I0toIi, map_camera_pre_integration_IitoIi1; //map_camera_cpi_I0toIi, map_camera_cpi_IitoIi1;
	for (auto const &time_pair : map_camera_times)
	{
		double current_time = time_pair.first;

		if (current_time == oldest_camera_time)
		{
			map_camera_pre_integration_I0toIi.insert({current_time, nullptr});
			map_camera_pre_integration_IitoIi1.insert({current_time, nullptr});
			last_camera_timestamp = current_time;
			continue;
		}

		double pre_integration_I0toIi1_time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
		double pre_integration_I0toIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_I0toIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb, initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_I0toIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_I0toIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_I0toIi1_time_0_in_imu, pre_integration_I0toIi1_time_1_in_imu);

		if (pre_integration_I0toIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)
				<< " in has " << pre_integration_I0toIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_I0toIi1_dt_imu = pre_integration_I0toIi1_readings.at(pre_integration_I0toIi1_readings.size() - 1).timestamp - pre_integration_I0toIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_I0toIi1_dt_imu - (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_I0toIi1_dt_imu << " of "
				<< (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_I0toIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_I0toIi1_readings.at(k);
			auto imu_1 = pre_integration_I0toIi1_readings.at(k + 1);
			pre_integration_I0toIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		double pre_integration_IitoIi1_time_0_in_imu = last_camera_timestamp + initializer_options.calib_camimu_dt;
		double pre_integration_IitoIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_IitoIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb,
			initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_IitoIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_IitoIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_IitoIi1_time_0_in_imu, pre_integration_IitoIi1_time_1_in_imu);

		if (pre_integration_IitoIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << " in has "
				<< pre_integration_IitoIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_IitoIi1_dt_imu = pre_integration_IitoIi1_readings.at(pre_integration_IitoIi1_readings.size() - 1).timestamp - pre_integration_IitoIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_IitoIi1_dt_imu - (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_IitoIi1_dt_imu << " of "
				<< (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_IitoIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_IitoIi1_readings.at(k);
			auto imu_1 = pre_integration_IitoIi1_readings.at(k + 1);
			pre_integration_IitoIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		map_camera_pre_integration_I0toIi.insert({current_time, pre_integration_I0toIi1});
		map_camera_pre_integration_IitoIi1.insert({current_time, pre_integration_IitoIi1});
		last_camera_timestamp = current_time;
	}

	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_measurements, system_size);
	Eigen::VectorXd b = Eigen::VectorXd::Zero(num_measurements);
	std::cout << "[initialize-dynamic]: System of " << num_measurements << " measurement x " << system_size << " states created (" << num_features
		<< " features, " << ((have_stereo) ? "stereo" : "mono") << ")" << std::endl;

	int index_meas = 0;
	int idx_feat = 0;
	std::map<size_t, int> A_index_features;

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (A_index_features.find(feat.first) == A_index_features.end())
		{
			A_index_features.insert({feat.first, idx_feat});
			idx_feat += 1;
		}

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);

				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_norm;
				uv_norm << (double)feat.second->uvs_norm.at(cam_id).at(i)(0), (double)feat.second->uvs_norm.at(cam_id).at(i)(1);

				double dt = 0.0;
				Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
				Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
				if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
				{
					dt = map_camera_pre_integration_I0toIi.at(time)->dt;
					R_I0toIk = map_camera_pre_integration_I0toIi.at(time)->R_tau_k;
					alpha_I0toIk = map_camera_pre_integration_I0toIi.at(time)->alpha_tau;
				}

				Eigen::MatrixXd H_proj = Eigen::MatrixXd::Zero(2, 3);
				H_proj << 1, 0, -uv_norm(0), 0, 1, -uv_norm(1);
				Eigen::MatrixXd Y = H_proj * R_ItoC * R_I0toIk;
				Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(2, system_size);
				Eigen::MatrixXd b_i = Y * alpha_I0toIk - H_proj * p_IinC;

				if (size_feature == 1)
				{
					assert(false);
				}
				else
				{
					H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y;
				}

				H_i.block(0, size_feature * num_features + 0, 2, 3) = -dt * Y;
				H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * dt * dt * Y;

				A.block(index_meas, 0, 2, A.cols()) = H_i;
				b.block(index_meas, 0, 2, 1) = b_i;
				index_meas += 2;
			}
		}
	}
	auto rT3 = boost::posix_time::microsec_clock::local_time();

	Eigen::MatrixXd A1 = A.block(0, 0, A.rows(), A.cols() - 3);
	Eigen::MatrixXd A1A1_inv = (A1.transpose() * A1).llt().solve(Eigen::MatrixXd::Identity(A1.cols(), A1.cols()));
	Eigen::MatrixXd A2 = A.block(0, A.cols() - 3, A.rows(), 3);
	Eigen::MatrixXd Temp = A2.transpose() * (Eigen::MatrixXd::Identity(A1.rows(), A1.rows()) - A1 * A1A1_inv * A1.transpose());
	Eigen::MatrixXd D = Temp * A2;
	Eigen::MatrixXd d = Temp * b;
	Eigen::Matrix<double, 7, 1> coeff = initializerHelper::computeDongsiCoeff(D, d, initializer_options.gravity_mag);

	assert(coeff(0) == 1);
	Eigen::Matrix<double, 6, 6> companion_matrix = Eigen::Matrix<double, 6, 6>::Zero(coeff.rows() - 1, coeff.rows() - 1);
	companion_matrix.diagonal(-1).setOnes();
	companion_matrix.col(companion_matrix.cols() - 1) = - coeff.reverse().head(coeff.rows() - 1);
	Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> svd_0(companion_matrix);
	Eigen::MatrixXd singular_values_0 = svd_0.singularValues();
	double cond_0 = singular_values_0(0) / singular_values_0(singular_values_0.rows() - 1);

	std::cout << std::fixed << "[initialize-dynamic]: CM cond = " << cond_0 << " | rank = " << (int)svd_0.rank() << " of " << (int)companion_matrix.cols()
		<< " (" << svd_0.threshold() << " thresh)" << std::endl;

	if (svd_0.rank() != companion_matrix.rows())
	{
		std::cout << "[initialize-dynamic]: Eigenvalue decomposition not full rank" << std::endl;
		return false;
	}

	Eigen::EigenSolver<Eigen::Matrix<double, 6, 6>> solver(companion_matrix, false);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "[initialize-dynamic]: failed to compute the eigenvalue decomposition" << std::endl;
		return false;
	}

	bool lambda_found = false;
	double lambda_min = -1;
	double cost_min = INFINITY;
	Eigen::MatrixXd I_dd = Eigen::MatrixXd::Identity(D.rows(), D.rows());

	for (int i = 0; i < solver.eigenvalues().size(); i++) {
		auto val = solver.eigenvalues()(i);
		if (val.imag() == 0)
		{
			double lambda = val.real();
			Eigen::MatrixXd D_lambdaI_inv = (D - lambda * I_dd).llt().solve(I_dd);
			Eigen::VectorXd state_grav = D_lambdaI_inv * d;
			double cost = std::abs(state_grav.norm() - initializer_options.gravity_mag);

			if (!lambda_found || cost < cost_min)
			{
				lambda_found = true;
				lambda_min = lambda;
				cost_min = cost;
			}
		}
	}

	if (!lambda_found)
	{
		std::cout << "[initialize-dynamic]: Failed to find a real eigenvalue" << std::endl;
		return false;
	}

	std::cout << std::fixed << "[initialize-dynamic]: Smallest real eigenvalue = " << lambda_min << " (cost of "
		<< cost_min << ")" << std::endl;

	Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).llt().solve(I_dd);
	Eigen::VectorXd state_grav = D_lambdaI_inv * d;

	Eigen::VectorXd state_feat_vel = - A1A1_inv * A1.transpose() * A2 * state_grav + A1A1_inv * A1.transpose() * b;
	Eigen::MatrixXd x_hat = Eigen::MatrixXd::Zero(system_size, 1);
	x_hat.block(0, 0, size_feature * num_features + 3, 1) = state_feat_vel;
	x_hat.block(size_feature * num_features + 3, 0, 3, 1) = state_grav;
	Eigen::Vector3d v_I0inI0 = x_hat.block(size_feature * num_features + 0, 0, 3, 1);
	std::cout << std::fixed << "[initialize-dynamic]: Velocity in I0 was " << v_I0inI0(0) << ", " << v_I0inI0(1) << ", " << v_I0inI0(2)
		<< " and |v| = " << v_I0inI0.norm() << std::endl;

	Eigen::Vector3d gravity_inI0 = x_hat.block(size_feature * num_features + 3, 0, 3, 1);
	double init_max_grav_difference = 1e-3;

	if (std::abs(gravity_inI0.norm() - initializer_options.gravity_mag) > init_max_grav_difference)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Gravity did not converge (" << std::abs(gravity_inI0.norm() - initializer_options.gravity_mag)
			<< " > " << init_max_grav_difference << ")" << std::endl;
		return false;
	}

	std::cout << "[initialize-dynamic]: Gravity in I0 was " << gravity_inI0(0) << ", " << gravity_inI0(1) << ", " << gravity_inI0(2) << " and |g| = " << gravity_inI0.norm() << std::endl;
	auto rT4 = boost::posix_time::microsec_clock::local_time();

	std::map<double, Eigen::VectorXd> ori_I0toIi, pos_IiinI0, vel_IiinI0;

	for (auto const &time_pair : map_camera_times)
	{
		double time = time_pair.first;

		double dt = 0.0;
		Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
		Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
		Eigen::MatrixXd beta_I0toIk = Eigen::MatrixXd::Zero(3, 1);

		if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
		{
			auto pre_integration = map_camera_pre_integration_I0toIi.at(time);
			dt = pre_integration->dt;
			R_I0toIk = pre_integration->R_tau_k;
			alpha_I0toIk = pre_integration->alpha_tau;
			beta_I0toIk = pre_integration->beta_tau;
		}

		Eigen::Vector3d p_IkinI0 = v_I0inI0 * dt - 0.5 * gravity_inI0 * dt * dt + alpha_I0toIk;
		Eigen::Vector3d v_IkinI0 = v_I0inI0 - gravity_inI0 * dt + beta_I0toIk;

		ori_I0toIi.insert({time, quatType::rotToQuat(R_I0toIk)});
		pos_IiinI0.insert({time, p_IkinI0});
		vel_IiinI0.insert({time, v_IkinI0});
	}

	count_valid_features = 0;
	std::map<size_t, Eigen::Vector3d> features_inI0;
	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		Eigen::Vector3d p_FinI0;
		if (size_feature == 1)
			assert(false);
		else
			p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first), 0, 3, 1);

		bool is_behind = false;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Vector3d p_FinC0 = quatType::quatToRot(q_ItoC) * p_FinI0 + p_IinC;

			if (p_FinC0(2) < 0) is_behind = true;
		}

		if (!is_behind)
		{
			features_inI0.insert({feat.first, p_FinI0});
			count_valid_features++;
		}
	}

	if (count_valid_features < min_valid_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Not enough features for our mle (" << count_valid_features << " < "
			<< min_valid_features << ")" << std::endl;
		return false;
	}

	Eigen::Matrix3d R_GtoI0;
	initializerHelper::gramSchmidt(gravity_inI0, R_GtoI0);
	Eigen::Vector4d q_GtoI0 = quatType::rotToQuat(R_GtoI0);
	Eigen::Vector3d gravity;
	gravity << 0.0, 0.0, initializer_options.gravity_mag;
	std::map<double, Eigen::VectorXd> ori_GtoIi, pos_IiinG, vel_IiinG;
	std::map<size_t, Eigen::Vector3d> features_inG;

	for (auto const &time_pair : map_camera_times)
	{
		ori_GtoIi[time_pair.first] = quatType::quatMultiply(ori_I0toIi.at(time_pair.first), q_GtoI0);
		pos_IiinG[time_pair.first] = R_GtoI0.transpose() * pos_IiinI0.at(time_pair.first);
		vel_IiinG[time_pair.first] = R_GtoI0.transpose() * vel_IiinI0.at(time_pair.first);
	}

	for (auto const &feat : features_inI0)
	features_inG[feat.first] = R_GtoI0.transpose() * feat.second;

	ceres::Problem problem;

	std::map<double, int> map_states;
	std::vector<double*> ceres_vars_ori;
	std::vector<double*> ceres_vars_pos;
	std::vector<double*> ceres_vars_vel;
	std::vector<double*> ceres_vars_bias_g;
	std::vector<double*> ceres_vars_bias_a;

	std::map<size_t, int> map_features;
	std::vector<double*> ceres_vars_feat;

	std::map<size_t, int> map_calib_cam2imu;
	std::vector<double*> ceres_vars_calib_cam2imu_ori;
	std::vector<double*> ceres_vars_calib_cam2imu_pos;

	std::map<size_t, int> map_calib_cam;
	std::vector<double*> ceres_vars_calib_cam_intrinsics;

	auto freeStateMemory = [&]()
	{
		for (auto const &ptr : ceres_vars_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_vel)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_g)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_a)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_feat)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam_intrinsics)
			delete[] ptr;
	};

	ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_SCHUR;
	options.trust_region_strategy_type = ceres::DOGLEG;
	options.num_threads = initializer_options.init_dyn_mle_max_threads;
	options.max_solver_time_in_seconds = initializer_options.init_dyn_mle_max_time;
	options.max_num_iterations = initializer_options.init_dyn_mle_max_iter;
	options.function_tolerance = 1e-5;
	options.gradient_tolerance = 1e-4 * options.function_tolerance;

	double timestamp_k = -1;

	for (auto const &time_pair : map_camera_times)
	{
		double timestamp_k1 = time_pair.first;
		std::shared_ptr<preIntegrationV1> pre_integration = map_camera_pre_integration_IitoIi1.at(timestamp_k1);
		Eigen::Matrix<double, 16, 1> state_k1;
		state_k1.block(0, 0, 4, 1) = ori_GtoIi.at(timestamp_k1);
		state_k1.block(4, 0, 3, 1) = pos_IiinG.at(timestamp_k1);
		state_k1.block(7, 0, 3, 1) = vel_IiinG.at(timestamp_k1);
		state_k1.block(10, 0, 3, 1) = gyroscope_bias;
		state_k1.block(13, 0, 3, 1) = accelerometer_bias;

		auto *var_ori = new double[4];
		for (int j = 0; j < 4; j++)
			var_ori[j] = state_k1(0 + j, 0);

		auto *var_pos = new double[3];
		auto *var_vel = new double[3];
		auto *var_bias_g = new double[3];
		auto *var_bias_a = new double[3];

		for (int j = 0; j < 3; j++)
		{
			var_pos[j] = state_k1(4 + j, 0);
			var_vel[j] = state_k1(7 + j, 0);
			var_bias_g[j] = state_k1(10 + j, 0);
			var_bias_a[j] = state_k1(13 + j, 0);
		}

		auto ceres_jplquat = new stateJplQuatLocal();
		problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
		problem.AddParameterBlock(var_pos, 3);
		problem.AddParameterBlock(var_vel, 3);
		problem.AddParameterBlock(var_bias_g, 3);
		problem.AddParameterBlock(var_bias_a, 3);

		if (map_states.empty())
		{
			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(13, 1);

			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_ori[j];

			for (int j = 0; j < 3; j++)
			{
				x_lin(4 + j) = var_pos[j];
				x_lin(7 + j) = var_bias_g[j];
				x_lin(10 + j) = var_bias_a[j];
			}

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(10, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(10, 10);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-5, 2);
			prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(0.05, 2);
			prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(0.10, 2);

			std::vector<std::string> x_types;
			std::vector<double *> factor_params;
			factor_params.push_back(var_ori);
			x_types.emplace_back("quat_yaw");
			factor_params.push_back(var_pos);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_g);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_a);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);
		}

		map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
		ceres_vars_ori.push_back(var_ori);
		ceres_vars_pos.push_back(var_pos);
		ceres_vars_vel.push_back(var_vel);
		ceres_vars_bias_g.push_back(var_bias_g);
		ceres_vars_bias_a.push_back(var_bias_a);

		if (pre_integration != nullptr)
		{
			assert(timestamp_k != -1);
			std::vector<double *> factor_params;
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
			auto *factor_imu = new factorImuPreIntegrationV1(pre_integration->dt, gravity, pre_integration->alpha_tau, pre_integration->beta_tau,
				pre_integration->q_tau_k, pre_integration->b_a_lin, pre_integration->b_w_lin, pre_integration->J_q, pre_integration->J_b,
				pre_integration->J_a, pre_integration->H_b, pre_integration->H_a, pre_integration->P_meas);

			problem.AddResidualBlock(factor_imu, nullptr, factor_params);
		}

		timestamp_k = timestamp_k1;
	}

	for (auto const &id_pair : map_camera_ids)
	{
		size_t cam_id = id_pair.first;
		if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end())
		{
			auto *var_calib_ori = new double[4];
			for (int j = 0; j < 4; j++)
				var_calib_ori[j] = initializer_options.camera_extrinsics.at(cam_id)(0 + j, 0);

			auto *var_calib_pos = new double[3];
			for (int j = 0; j < 3; j++)
				var_calib_pos[j] = initializer_options.camera_extrinsics.at(cam_id)(4 + j, 0);

			auto ceres_calib_jplquat = new stateJplQuatLocal();
			problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
			problem.AddParameterBlock(var_calib_pos, 3);

			map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
			ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
			ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_calib_ori[j];

			for (int j = 0; j < 3; j++)
				x_lin(4 + j) = var_calib_pos[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
			prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
			prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_ori);
			x_types.emplace_back("quat");
			factor_params.push_back(var_calib_pos);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
			{
				problem.SetParameterBlockConstant(var_calib_ori);
				problem.SetParameterBlockConstant(var_calib_pos);
			}
		}

		if (map_calib_cam.find(cam_id) == map_calib_cam.end())
		{
			auto *var_calib_cam = new double[8];
			for (int j = 0; j < 8; j++)
				var_calib_cam[j] = initializer_options.camera_intrinsics.at(cam_id)->getValue()(j, 0);

			problem.AddParameterBlock(var_calib_cam, 8);
			map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
			ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
			for (int j = 0; j < 8; j++)
				x_lin(0 + j) = var_calib_cam[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
			prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_cam);
			x_types.emplace_back("vec8");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
				problem.SetParameterBlockConstant(var_calib_cam);
		}
	}
	assert(map_calib_cam2imu.size() == map_calib_cam.size());

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (features_inG.find(feat.first) == features_inG.end()) continue;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t feat_id = feat.first;
			size_t cam_id = cam_time.first;
			bool is_fisheye = (std::dynamic_pointer_cast<cameraEqui>(initializer_options.camera_intrinsics.at(cam_id)) != nullptr);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);
				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_raw = feat.second->uvs.at(cam_id).at(i).block(0, 0, 2, 1).cast<double>();

				if (map_features.find(feat_id) == map_features.end())
				{
					auto *var_feat = new double[3];
					for (int j = 0; j < 3; j++)
						var_feat[j] = features_inG.at(feat_id)(j);

					problem.AddParameterBlock(var_feat, 3);
					map_features.insert({feat_id, (int)ceres_vars_feat.size()});
					ceres_vars_feat.push_back(var_feat);
				}

				std::vector<double *> factor_params;
				factor_params.push_back(ceres_vars_ori.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_pos.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_feat.at(map_features.at(feat_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
				auto *factor_pinhole = new factorImageReprojCalib(uv_raw, initializer_options.sigma_pix, is_fisheye);

				ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
				problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
			}
		}
	}
	assert(ceres_vars_ori.size() == ceres_vars_bias_g.size());
	assert(ceres_vars_ori.size() == ceres_vars_vel.size());
	assert(ceres_vars_ori.size() == ceres_vars_bias_a.size());
	assert(ceres_vars_ori.size() == ceres_vars_pos.size());
	auto rT5 = boost::posix_time::microsec_clock::local_time();

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	std::cout << "[initialize-dynamic]: " << (int)summary.iterations.size() << " iterations | " << map_states.size() << " states, "
		<< map_features.size() << " feats (" << count_valid_features << " valid) | " << summary.num_parameters << " param and "
		<< summary.num_residuals << " res | cost " << std::fixed << summary.initial_cost << " => " << summary.final_cost << std::endl;

	auto rT6 = boost::posix_time::microsec_clock::local_time();

	timestamp = newest_cam_time;
	if (initializer_options.init_dyn_mle_max_iter != 0 && summary.termination_type != ceres::CONVERGENCE)
	{
		std::cout << "[initialize-dynamic]: Optimization failed: " << summary.message << std::endl;
		freeStateMemory();
		return false;
	}
	std::cout << "[initialize-dynamic]: " << summary.message << std::endl;

	auto getPose = [&](double timestamp)
	{
		Eigen::VectorXd state_imu = Eigen::VectorXd::Zero(16);

		for (int i = 0; i < 4; i++)
			state_imu(0 + i) = ceres_vars_ori[map_states[timestamp]][i];

		for (int i = 0; i < 3; i++)
		{
			state_imu(4 + i) = ceres_vars_pos[map_states[timestamp]][i];
			state_imu(7 + i) = ceres_vars_vel[map_states[timestamp]][i];
			state_imu(10 + i) = ceres_vars_bias_g[map_states[timestamp]][i];
			state_imu(13 + i) = ceres_vars_bias_a[map_states[timestamp]][i];
		}

		return state_imu;
	};

	assert(map_states.find(newest_cam_time) != map_states.end());

	if (imu_state_ == nullptr)
		imu_state_ = std::make_shared<imuState>();

	Eigen::VectorXd imu_state = getPose(newest_cam_time);
	imu_state_->setValue(imu_state);
	imu_state_->setFej(imu_state);

	for (auto const &state_pair : map_states)
	{
		Eigen::VectorXd pose = getPose(state_pair.first);

		if (clones_imu_.find(state_pair.first) == clones_imu_.end())
		{
			auto pose_ = std::make_shared<poseJpl>();
			pose_->setValue(pose.block(0, 0, 7, 1));
			pose_->setFej(pose.block(0, 0, 7, 1));
			clones_imu_.insert({state_pair.first, pose_});
		}
		else
		{
			clones_imu_.at(state_pair.first)->setValue(pose.block(0, 0, 7, 1));
			clones_imu_.at(state_pair.first)->setFej(pose.block(0, 0, 7, 1));
		}
	}

	for (auto const &feat_pair : map_features)
	{
		Eigen::Vector3d feature;
		feature << ceres_vars_feat[feat_pair.second][0], ceres_vars_feat[feat_pair.second][1], ceres_vars_feat[feat_pair.second][2];

		if (map_points_.find(feat_pair.first) == map_points_.end())
		{
			auto feature_ = std::make_shared<mapPoint>();
			feature_->feature_id = feat_pair.first;
			feature_->setPointXYZ(feature, false);
			feature_->setPointXYZ(feature, true);
			map_points_.insert({feat_pair.first, feature_});
		}
		else
		{
			map_points_.at(feat_pair.first)->feature_id = feat_pair.first;
			map_points_.at(feat_pair.first)->setPointXYZ(feature, false);
			map_points_.at(feat_pair.first)->setPointXYZ(feature, true);
		}
	}

	std::vector<std::pair<const double *, const double *>> covariance_blocks;
	int state_index = map_states[newest_cam_time];

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_ori[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index]));

	ceres::Covariance::Options options_cov;
	options_cov.null_space_rank = (!initializer_options.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8));
	options_cov.min_reciprocal_condition_number = initializer_options.init_dyn_min_rec_cond;

	options_cov.apply_loss_function = true;
	options_cov.num_threads = initializer_options.init_dyn_mle_max_threads;

	ceres::Covariance problem_cov(options_cov);
	bool success = problem_cov.Compute(covariance_blocks, &problem);

	if (!success)
	{
		std::cout << "[initialize-dynamic]: Covariance recovery failed..." << std::endl;
		freeStateMemory();
		return false;
	}

	order.clear();
	order.push_back(imu_state_);
	covariance = Eigen::MatrixXd::Zero(imu_state_->getSize(), imu_state_->getSize());
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> cov_tmp = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], cov_tmp.data()));
	covariance.block(0, 0, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(3, 3, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(6, 6, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(9, 9, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(12, 12, 3, 3) = cov_tmp.eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(0, 3, 3, 3) = cov_tmp.eval();
	covariance.block(3, 0, 3, 3) = cov_tmp.transpose();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(0, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(0, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(0, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 0, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(3, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(3, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(3, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 3, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(6, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 6, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(6, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 6, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(9, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 9, 3, 3) = cov_tmp.transpose().eval();

	covariance.block(0, 0, 3, 3) *= initializer_options.init_dyn_inflation_orientation;
	covariance.block(6, 6, 3, 3) *= initializer_options.init_dyn_inflation_velocity;
	covariance.block(9, 9, 3, 3) *= initializer_options.init_dyn_inflation_bias_gyro;
	covariance.block(12, 12, 3, 3) *= initializer_options.init_dyn_inflation_bias_accel;

	covariance = 0.5 * (covariance + covariance.transpose());
	Eigen::Vector3d sigmas_vel = covariance.block(6, 6, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_bg = covariance.block(9, 9, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_ba = covariance.block(12, 12, 3, 3).diagonal().transpose().cwiseSqrt();
	std::cout << "[initialize-dynamic]: vel priors = " << sigmas_vel(0) << ", " << sigmas_vel(1) << ", " << sigmas_vel(2) << std::endl;
	std::cout << "[initialize-dynamic]: bg priors = " << sigmas_bg(0) << ", " << sigmas_bg(1) << ", " << sigmas_bg(2) << std::endl;
	std::cout << "[initialize-dynamic]: ba priors = " << sigmas_ba(0) << ", " << sigmas_ba(1) << ", " << sigmas_ba(2) << std::endl;

	Eigen::MatrixXd x = imu_state_->value();
	x.block(4, 0, 3, 1).setZero();
	imu_state_->setValue(x);
	imu_state_->setFej(x);

	auto rT7 = boost::posix_time::microsec_clock::local_time();
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " sec for prelim tests" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " sec for linsys setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT4 - rT3).total_microseconds() * 1e-6 << " sec for linsys" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT5 - rT4).total_microseconds() * 1e-6 << " sec for ceres opt setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT6 - rT5).total_microseconds() * 1e-6 << " sec for ceres opt" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT6).total_microseconds() * 1e-6 << " sec for ceres covariance" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT1).total_microseconds() * 1e-6 << " sec total for initialization" << std::endl;
	freeStateMemory();

	return true;
}
  protected:
    inertialInitializerOptions initializer_options;
    std::shared_ptr<featureDatabase> db;
    std::shared_ptr<std::vector<imuData>> imu_data;
    std::shared_ptr<staticInitializer> static_initializer;
    std::shared_ptr<dynamicInitializer> dynamic_initializer;
  public:
    explicit staticInitializer(inertialInitializerOptions &initializer_options_, std::shared_ptr<featureDatabase> db_,
    std::shared_ptr<std::vector<imuData>> imu_data_) {

}
    bool initialize(double &timestamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<baseType>> &order,
    std::shared_ptr<imuState> imu_state_, bool wait_for_jerk = true) {
	auto rT1 = boost::posix_time::microsec_clock::local_time();
	double newest_cam_time = -1;
	for (auto const &feat : db->getInternalData())
	{
		for (auto const &cam_time_pair : feat.second->timestamps)
		{
			for (auto const &time : cam_time_pair.second)
			{
				newest_cam_time = std::max(newest_cam_time, time);
			}
		}
	}

	double oldest_time = newest_cam_time - initializer_options.init_window_time;

	if (newest_cam_time < 0 || oldest_time < 0) return false;

	db->cleanUpOldmeasurements(oldest_time);

	bool have_old_imu_readings = false;
	auto it_imu = imu_data->begin();

	while (it_imu != imu_data->end() && it_imu->timestamp < oldest_time + initializer_options.calib_camimu_dt)
	{
		have_old_imu_readings = true;
		it_imu = imu_data->erase(it_imu);
	}

	if (db->getInternalData().size() < 0.75 * initializer_options.init_max_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Only " << db->getInternalData().size() << " valid features of required ("
			<< 0.95 * initializer_options.init_max_features << " thresh)" << std::endl;
		return false;
	}

	if (imu_data->size() < 2 || !have_old_imu_readings)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Waiting for window to reach full size (" << imu_data->size() << " imu readings)" << std::endl;
		return false;
	}

	std::unordered_map<size_t, std::shared_ptr<feature>> features;

	for (const auto &feat : db->getInternalData())
	{
		auto feat_new = std::make_shared<feature>();
		feat_new->feature_id = feat.second->feature_id;
		feat_new->uvs = feat.second->uvs;
		feat_new->uvs_norm = feat.second->uvs_norm;
		feat_new->timestamps = feat.second->timestamps;
		features.insert({feat.first, feat_new});
	}

	const int min_num_meas_to_optimize = (int)initializer_options.init_window_time;
	const int min_valid_features = 8;

	bool have_stereo = false;
	int count_valid_features = 0;
	std::map<size_t, int> map_features_num_meas;
	int num_measurements = 0;
	double oldest_camera_time = INFINITY;
	std::map<double, bool> map_camera_times;
	map_camera_times[newest_cam_time] = true;
	std::map<size_t, bool> map_camera_ids;
	double pose_dt_avg = initializer_options.init_window_time / (double)(initializer_options.init_dyn_num_pose + 1);

	for (auto const &feat : features)
	{
		std::vector<double> times;
		std::map<size_t, bool> cam_ids;

		for (auto const &cam_time : feat.second->timestamps)
		{
			for (double time : cam_time.second)
			{
				double time_dt = INFINITY;

				for (auto const &tmp : map_camera_times)
					time_dt = std::min(time_dt, std::abs(time - tmp.first));

				for (auto const &tmp : times)
					time_dt = std::min(time_dt, std::abs(time - tmp));

				if (time_dt >= pose_dt_avg || time_dt == 0.0)
				{
					times.push_back(time);
					cam_ids[cam_time.first] = true;
				}
			}
		}

		map_features_num_meas[feat.first] = (int)times.size();
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		for (auto const &tmp : times)
		{
			map_camera_times[tmp] = true;
			oldest_camera_time = std::min(oldest_camera_time, tmp);
			num_measurements += 2;
		}

		for (auto const &tmp : cam_ids)
			map_camera_ids[tmp.first] = true;

		if (cam_ids.size() > 1) have_stereo = true;

		count_valid_features++;
	}

	if ((int)map_camera_times.size() < initializer_options.init_dyn_num_pose)
		return false;

	if (count_valid_features < min_valid_features)
	{
		std::cout << "[initialize-dynamic]: Only " << count_valid_features << " valid features of required " << min_valid_features << std::endl;
		return false;
	}

	Eigen::Vector3d gyroscope_bias = initializer_options.init_dyn_bias_g;
	Eigen::Vector3d accelerometer_bias = initializer_options.init_dyn_bias_a;

	double accel_inI_norm = 0.0;
	double theta_inI_norm = 0.0;
	double time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
	double time_1_in_imu = newest_cam_time + initializer_options.calib_camimu_dt;
	std::vector<imuData> readings = initializerHelper::selectImuReadings(*imu_data, time_0_in_imu, time_1_in_imu);

	assert(readings.size() > 2);
	for (size_t k = 0; k < readings.size() - 1; k++)
	{
		auto imu_0 = readings.at(k);
		auto imu_1 = readings.at(k + 1);
		double dt = imu_1.timestamp - imu_0.timestamp;
		Eigen::Vector3d gyr = 0.5 * (imu_0.gyr + imu_1.gyr) - gyroscope_bias;
		Eigen::Vector3d acc = 0.5 * (imu_0.acc + imu_1.acc) - accelerometer_bias;
		theta_inI_norm += ( -gyr * dt).norm();
		accel_inI_norm += acc.norm();
	}

	accel_inI_norm /= (double)(readings.size() - 1);

	if (180.0 / M_PI * theta_inI_norm < initializer_options.init_dyn_min_deg)
	{
		std::cout << "[initialize-dynamic]: Gyroscope only " << 180.0 / M_PI * theta_inI_norm << " degree change ("
			<< initializer_options.init_dyn_min_deg << " thresh)" << std::endl;
		return false;
	}
	std::cout << std::fixed << "[initialize-dynamic]: |theta_I| = " << 180.0 / M_PI * theta_inI_norm
		<< " deg and |accel| = " << accel_inI_norm << std::endl;

 	auto rT2 = boost::posix_time::microsec_clock::local_time();

	const bool use_single_depth = false;
	int size_feature = (use_single_depth) ? 1 : 3;
	int num_features = count_valid_features;
	int system_size = size_feature * num_features + 3 + 3;

	if (num_measurements < system_size)
	{
		std::cout << "[initialize-dynamic]: Not enough feature measurements (" << num_measurements << " meas vs " << system_size
			<< " state size)" << std::endl;

		return false;
	}

	assert(oldest_camera_time < newest_cam_time);
	double last_camera_timestamp = 0.0;
	std::map<double, std::shared_ptr<preIntegrationV1>> map_camera_pre_integration_I0toIi, map_camera_pre_integration_IitoIi1; //map_camera_cpi_I0toIi, map_camera_cpi_IitoIi1;
	for (auto const &time_pair : map_camera_times)
	{
		double current_time = time_pair.first;

		if (current_time == oldest_camera_time)
		{
			map_camera_pre_integration_I0toIi.insert({current_time, nullptr});
			map_camera_pre_integration_IitoIi1.insert({current_time, nullptr});
			last_camera_timestamp = current_time;
			continue;
		}

		double pre_integration_I0toIi1_time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
		double pre_integration_I0toIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_I0toIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb, initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_I0toIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_I0toIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_I0toIi1_time_0_in_imu, pre_integration_I0toIi1_time_1_in_imu);

		if (pre_integration_I0toIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)
				<< " in has " << pre_integration_I0toIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_I0toIi1_dt_imu = pre_integration_I0toIi1_readings.at(pre_integration_I0toIi1_readings.size() - 1).timestamp - pre_integration_I0toIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_I0toIi1_dt_imu - (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_I0toIi1_dt_imu << " of "
				<< (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_I0toIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_I0toIi1_readings.at(k);
			auto imu_1 = pre_integration_I0toIi1_readings.at(k + 1);
			pre_integration_I0toIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		double pre_integration_IitoIi1_time_0_in_imu = last_camera_timestamp + initializer_options.calib_camimu_dt;
		double pre_integration_IitoIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_IitoIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb,
			initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_IitoIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_IitoIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_IitoIi1_time_0_in_imu, pre_integration_IitoIi1_time_1_in_imu);

		if (pre_integration_IitoIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << " in has "
				<< pre_integration_IitoIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_IitoIi1_dt_imu = pre_integration_IitoIi1_readings.at(pre_integration_IitoIi1_readings.size() - 1).timestamp - pre_integration_IitoIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_IitoIi1_dt_imu - (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_IitoIi1_dt_imu << " of "
				<< (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_IitoIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_IitoIi1_readings.at(k);
			auto imu_1 = pre_integration_IitoIi1_readings.at(k + 1);
			pre_integration_IitoIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		map_camera_pre_integration_I0toIi.insert({current_time, pre_integration_I0toIi1});
		map_camera_pre_integration_IitoIi1.insert({current_time, pre_integration_IitoIi1});
		last_camera_timestamp = current_time;
	}

	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_measurements, system_size);
	Eigen::VectorXd b = Eigen::VectorXd::Zero(num_measurements);
	std::cout << "[initialize-dynamic]: System of " << num_measurements << " measurement x " << system_size << " states created (" << num_features
		<< " features, " << ((have_stereo) ? "stereo" : "mono") << ")" << std::endl;

	int index_meas = 0;
	int idx_feat = 0;
	std::map<size_t, int> A_index_features;

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (A_index_features.find(feat.first) == A_index_features.end())
		{
			A_index_features.insert({feat.first, idx_feat});
			idx_feat += 1;
		}

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);

				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_norm;
				uv_norm << (double)feat.second->uvs_norm.at(cam_id).at(i)(0), (double)feat.second->uvs_norm.at(cam_id).at(i)(1);

				double dt = 0.0;
				Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
				Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
				if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
				{
					dt = map_camera_pre_integration_I0toIi.at(time)->dt;
					R_I0toIk = map_camera_pre_integration_I0toIi.at(time)->R_tau_k;
					alpha_I0toIk = map_camera_pre_integration_I0toIi.at(time)->alpha_tau;
				}

				Eigen::MatrixXd H_proj = Eigen::MatrixXd::Zero(2, 3);
				H_proj << 1, 0, -uv_norm(0), 0, 1, -uv_norm(1);
				Eigen::MatrixXd Y = H_proj * R_ItoC * R_I0toIk;
				Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(2, system_size);
				Eigen::MatrixXd b_i = Y * alpha_I0toIk - H_proj * p_IinC;

				if (size_feature == 1)
				{
					assert(false);
				}
				else
				{
					H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y;
				}

				H_i.block(0, size_feature * num_features + 0, 2, 3) = -dt * Y;
				H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * dt * dt * Y;

				A.block(index_meas, 0, 2, A.cols()) = H_i;
				b.block(index_meas, 0, 2, 1) = b_i;
				index_meas += 2;
			}
		}
	}
	auto rT3 = boost::posix_time::microsec_clock::local_time();

	Eigen::MatrixXd A1 = A.block(0, 0, A.rows(), A.cols() - 3);
	Eigen::MatrixXd A1A1_inv = (A1.transpose() * A1).llt().solve(Eigen::MatrixXd::Identity(A1.cols(), A1.cols()));
	Eigen::MatrixXd A2 = A.block(0, A.cols() - 3, A.rows(), 3);
	Eigen::MatrixXd Temp = A2.transpose() * (Eigen::MatrixXd::Identity(A1.rows(), A1.rows()) - A1 * A1A1_inv * A1.transpose());
	Eigen::MatrixXd D = Temp * A2;
	Eigen::MatrixXd d = Temp * b;
	Eigen::Matrix<double, 7, 1> coeff = initializerHelper::computeDongsiCoeff(D, d, initializer_options.gravity_mag);

	assert(coeff(0) == 1);
	Eigen::Matrix<double, 6, 6> companion_matrix = Eigen::Matrix<double, 6, 6>::Zero(coeff.rows() - 1, coeff.rows() - 1);
	companion_matrix.diagonal(-1).setOnes();
	companion_matrix.col(companion_matrix.cols() - 1) = - coeff.reverse().head(coeff.rows() - 1);
	Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> svd_0(companion_matrix);
	Eigen::MatrixXd singular_values_0 = svd_0.singularValues();
	double cond_0 = singular_values_0(0) / singular_values_0(singular_values_0.rows() - 1);

	std::cout << std::fixed << "[initialize-dynamic]: CM cond = " << cond_0 << " | rank = " << (int)svd_0.rank() << " of " << (int)companion_matrix.cols()
		<< " (" << svd_0.threshold() << " thresh)" << std::endl;

	if (svd_0.rank() != companion_matrix.rows())
	{
		std::cout << "[initialize-dynamic]: Eigenvalue decomposition not full rank" << std::endl;
		return false;
	}

	Eigen::EigenSolver<Eigen::Matrix<double, 6, 6>> solver(companion_matrix, false);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "[initialize-dynamic]: failed to compute the eigenvalue decomposition" << std::endl;
		return false;
	}

	bool lambda_found = false;
	double lambda_min = -1;
	double cost_min = INFINITY;
	Eigen::MatrixXd I_dd = Eigen::MatrixXd::Identity(D.rows(), D.rows());

	for (int i = 0; i < solver.eigenvalues().size(); i++) {
		auto val = solver.eigenvalues()(i);
		if (val.imag() == 0)
		{
			double lambda = val.real();
			Eigen::MatrixXd D_lambdaI_inv = (D - lambda * I_dd).llt().solve(I_dd);
			Eigen::VectorXd state_grav = D_lambdaI_inv * d;
			double cost = std::abs(state_grav.norm() - initializer_options.gravity_mag);

			if (!lambda_found || cost < cost_min)
			{
				lambda_found = true;
				lambda_min = lambda;
				cost_min = cost;
			}
		}
	}

	if (!lambda_found)
	{
		std::cout << "[initialize-dynamic]: Failed to find a real eigenvalue" << std::endl;
		return false;
	}

	std::cout << std::fixed << "[initialize-dynamic]: Smallest real eigenvalue = " << lambda_min << " (cost of "
		<< cost_min << ")" << std::endl;

	Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).llt().solve(I_dd);
	Eigen::VectorXd state_grav = D_lambdaI_inv * d;

	Eigen::VectorXd state_feat_vel = - A1A1_inv * A1.transpose() * A2 * state_grav + A1A1_inv * A1.transpose() * b;
	Eigen::MatrixXd x_hat = Eigen::MatrixXd::Zero(system_size, 1);
	x_hat.block(0, 0, size_feature * num_features + 3, 1) = state_feat_vel;
	x_hat.block(size_feature * num_features + 3, 0, 3, 1) = state_grav;
	Eigen::Vector3d v_I0inI0 = x_hat.block(size_feature * num_features + 0, 0, 3, 1);
	std::cout << std::fixed << "[initialize-dynamic]: Velocity in I0 was " << v_I0inI0(0) << ", " << v_I0inI0(1) << ", " << v_I0inI0(2)
		<< " and |v| = " << v_I0inI0.norm() << std::endl;

	Eigen::Vector3d gravity_inI0 = x_hat.block(size_feature * num_features + 3, 0, 3, 1);
	double init_max_grav_difference = 1e-3;

	if (std::abs(gravity_inI0.norm() - initializer_options.gravity_mag) > init_max_grav_difference)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Gravity did not converge (" << std::abs(gravity_inI0.norm() - initializer_options.gravity_mag)
			<< " > " << init_max_grav_difference << ")" << std::endl;
		return false;
	}

	std::cout << "[initialize-dynamic]: Gravity in I0 was " << gravity_inI0(0) << ", " << gravity_inI0(1) << ", " << gravity_inI0(2) << " and |g| = " << gravity_inI0.norm() << std::endl;
	auto rT4 = boost::posix_time::microsec_clock::local_time();

	std::map<double, Eigen::VectorXd> ori_I0toIi, pos_IiinI0, vel_IiinI0;

	for (auto const &time_pair : map_camera_times)
	{
		double time = time_pair.first;

		double dt = 0.0;
		Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
		Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
		Eigen::MatrixXd beta_I0toIk = Eigen::MatrixXd::Zero(3, 1);

		if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
		{
			auto pre_integration = map_camera_pre_integration_I0toIi.at(time);
			dt = pre_integration->dt;
			R_I0toIk = pre_integration->R_tau_k;
			alpha_I0toIk = pre_integration->alpha_tau;
			beta_I0toIk = pre_integration->beta_tau;
		}

		Eigen::Vector3d p_IkinI0 = v_I0inI0 * dt - 0.5 * gravity_inI0 * dt * dt + alpha_I0toIk;
		Eigen::Vector3d v_IkinI0 = v_I0inI0 - gravity_inI0 * dt + beta_I0toIk;

		ori_I0toIi.insert({time, quatType::rotToQuat(R_I0toIk)});
		pos_IiinI0.insert({time, p_IkinI0});
		vel_IiinI0.insert({time, v_IkinI0});
	}

	count_valid_features = 0;
	std::map<size_t, Eigen::Vector3d> features_inI0;
	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		Eigen::Vector3d p_FinI0;
		if (size_feature == 1)
			assert(false);
		else
			p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first), 0, 3, 1);

		bool is_behind = false;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Vector3d p_FinC0 = quatType::quatToRot(q_ItoC) * p_FinI0 + p_IinC;

			if (p_FinC0(2) < 0) is_behind = true;
		}

		if (!is_behind)
		{
			features_inI0.insert({feat.first, p_FinI0});
			count_valid_features++;
		}
	}

	if (count_valid_features < min_valid_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Not enough features for our mle (" << count_valid_features << " < "
			<< min_valid_features << ")" << std::endl;
		return false;
	}

	Eigen::Matrix3d R_GtoI0;
	initializerHelper::gramSchmidt(gravity_inI0, R_GtoI0);
	Eigen::Vector4d q_GtoI0 = quatType::rotToQuat(R_GtoI0);
	Eigen::Vector3d gravity;
	gravity << 0.0, 0.0, initializer_options.gravity_mag;
	std::map<double, Eigen::VectorXd> ori_GtoIi, pos_IiinG, vel_IiinG;
	std::map<size_t, Eigen::Vector3d> features_inG;

	for (auto const &time_pair : map_camera_times)
	{
		ori_GtoIi[time_pair.first] = quatType::quatMultiply(ori_I0toIi.at(time_pair.first), q_GtoI0);
		pos_IiinG[time_pair.first] = R_GtoI0.transpose() * pos_IiinI0.at(time_pair.first);
		vel_IiinG[time_pair.first] = R_GtoI0.transpose() * vel_IiinI0.at(time_pair.first);
	}

	for (auto const &feat : features_inI0)
	features_inG[feat.first] = R_GtoI0.transpose() * feat.second;

	ceres::Problem problem;

	std::map<double, int> map_states;
	std::vector<double*> ceres_vars_ori;
	std::vector<double*> ceres_vars_pos;
	std::vector<double*> ceres_vars_vel;
	std::vector<double*> ceres_vars_bias_g;
	std::vector<double*> ceres_vars_bias_a;

	std::map<size_t, int> map_features;
	std::vector<double*> ceres_vars_feat;

	std::map<size_t, int> map_calib_cam2imu;
	std::vector<double*> ceres_vars_calib_cam2imu_ori;
	std::vector<double*> ceres_vars_calib_cam2imu_pos;

	std::map<size_t, int> map_calib_cam;
	std::vector<double*> ceres_vars_calib_cam_intrinsics;

	auto freeStateMemory = [&]()
	{
		for (auto const &ptr : ceres_vars_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_vel)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_g)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_a)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_feat)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam_intrinsics)
			delete[] ptr;
	};

	ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_SCHUR;
	options.trust_region_strategy_type = ceres::DOGLEG;
	options.num_threads = initializer_options.init_dyn_mle_max_threads;
	options.max_solver_time_in_seconds = initializer_options.init_dyn_mle_max_time;
	options.max_num_iterations = initializer_options.init_dyn_mle_max_iter;
	options.function_tolerance = 1e-5;
	options.gradient_tolerance = 1e-4 * options.function_tolerance;

	double timestamp_k = -1;

	for (auto const &time_pair : map_camera_times)
	{
		double timestamp_k1 = time_pair.first;
		std::shared_ptr<preIntegrationV1> pre_integration = map_camera_pre_integration_IitoIi1.at(timestamp_k1);
		Eigen::Matrix<double, 16, 1> state_k1;
		state_k1.block(0, 0, 4, 1) = ori_GtoIi.at(timestamp_k1);
		state_k1.block(4, 0, 3, 1) = pos_IiinG.at(timestamp_k1);
		state_k1.block(7, 0, 3, 1) = vel_IiinG.at(timestamp_k1);
		state_k1.block(10, 0, 3, 1) = gyroscope_bias;
		state_k1.block(13, 0, 3, 1) = accelerometer_bias;

		auto *var_ori = new double[4];
		for (int j = 0; j < 4; j++)
			var_ori[j] = state_k1(0 + j, 0);

		auto *var_pos = new double[3];
		auto *var_vel = new double[3];
		auto *var_bias_g = new double[3];
		auto *var_bias_a = new double[3];

		for (int j = 0; j < 3; j++)
		{
			var_pos[j] = state_k1(4 + j, 0);
			var_vel[j] = state_k1(7 + j, 0);
			var_bias_g[j] = state_k1(10 + j, 0);
			var_bias_a[j] = state_k1(13 + j, 0);
		}

		auto ceres_jplquat = new stateJplQuatLocal();
		problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
		problem.AddParameterBlock(var_pos, 3);
		problem.AddParameterBlock(var_vel, 3);
		problem.AddParameterBlock(var_bias_g, 3);
		problem.AddParameterBlock(var_bias_a, 3);

		if (map_states.empty())
		{
			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(13, 1);

			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_ori[j];

			for (int j = 0; j < 3; j++)
			{
				x_lin(4 + j) = var_pos[j];
				x_lin(7 + j) = var_bias_g[j];
				x_lin(10 + j) = var_bias_a[j];
			}

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(10, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(10, 10);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-5, 2);
			prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(0.05, 2);
			prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(0.10, 2);

			std::vector<std::string> x_types;
			std::vector<double *> factor_params;
			factor_params.push_back(var_ori);
			x_types.emplace_back("quat_yaw");
			factor_params.push_back(var_pos);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_g);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_a);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);
		}

		map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
		ceres_vars_ori.push_back(var_ori);
		ceres_vars_pos.push_back(var_pos);
		ceres_vars_vel.push_back(var_vel);
		ceres_vars_bias_g.push_back(var_bias_g);
		ceres_vars_bias_a.push_back(var_bias_a);

		if (pre_integration != nullptr)
		{
			assert(timestamp_k != -1);
			std::vector<double *> factor_params;
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
			auto *factor_imu = new factorImuPreIntegrationV1(pre_integration->dt, gravity, pre_integration->alpha_tau, pre_integration->beta_tau,
				pre_integration->q_tau_k, pre_integration->b_a_lin, pre_integration->b_w_lin, pre_integration->J_q, pre_integration->J_b,
				pre_integration->J_a, pre_integration->H_b, pre_integration->H_a, pre_integration->P_meas);

			problem.AddResidualBlock(factor_imu, nullptr, factor_params);
		}

		timestamp_k = timestamp_k1;
	}

	for (auto const &id_pair : map_camera_ids)
	{
		size_t cam_id = id_pair.first;
		if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end())
		{
			auto *var_calib_ori = new double[4];
			for (int j = 0; j < 4; j++)
				var_calib_ori[j] = initializer_options.camera_extrinsics.at(cam_id)(0 + j, 0);

			auto *var_calib_pos = new double[3];
			for (int j = 0; j < 3; j++)
				var_calib_pos[j] = initializer_options.camera_extrinsics.at(cam_id)(4 + j, 0);

			auto ceres_calib_jplquat = new stateJplQuatLocal();
			problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
			problem.AddParameterBlock(var_calib_pos, 3);

			map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
			ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
			ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_calib_ori[j];

			for (int j = 0; j < 3; j++)
				x_lin(4 + j) = var_calib_pos[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
			prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
			prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_ori);
			x_types.emplace_back("quat");
			factor_params.push_back(var_calib_pos);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
			{
				problem.SetParameterBlockConstant(var_calib_ori);
				problem.SetParameterBlockConstant(var_calib_pos);
			}
		}

		if (map_calib_cam.find(cam_id) == map_calib_cam.end())
		{
			auto *var_calib_cam = new double[8];
			for (int j = 0; j < 8; j++)
				var_calib_cam[j] = initializer_options.camera_intrinsics.at(cam_id)->getValue()(j, 0);

			problem.AddParameterBlock(var_calib_cam, 8);
			map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
			ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
			for (int j = 0; j < 8; j++)
				x_lin(0 + j) = var_calib_cam[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
			prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_cam);
			x_types.emplace_back("vec8");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
				problem.SetParameterBlockConstant(var_calib_cam);
		}
	}
	assert(map_calib_cam2imu.size() == map_calib_cam.size());

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (features_inG.find(feat.first) == features_inG.end()) continue;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t feat_id = feat.first;
			size_t cam_id = cam_time.first;
			bool is_fisheye = (std::dynamic_pointer_cast<cameraEqui>(initializer_options.camera_intrinsics.at(cam_id)) != nullptr);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);
				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_raw = feat.second->uvs.at(cam_id).at(i).block(0, 0, 2, 1).cast<double>();

				if (map_features.find(feat_id) == map_features.end())
				{
					auto *var_feat = new double[3];
					for (int j = 0; j < 3; j++)
						var_feat[j] = features_inG.at(feat_id)(j);

					problem.AddParameterBlock(var_feat, 3);
					map_features.insert({feat_id, (int)ceres_vars_feat.size()});
					ceres_vars_feat.push_back(var_feat);
				}

				std::vector<double *> factor_params;
				factor_params.push_back(ceres_vars_ori.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_pos.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_feat.at(map_features.at(feat_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
				auto *factor_pinhole = new factorImageReprojCalib(uv_raw, initializer_options.sigma_pix, is_fisheye);

				ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
				problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
			}
		}
	}
	assert(ceres_vars_ori.size() == ceres_vars_bias_g.size());
	assert(ceres_vars_ori.size() == ceres_vars_vel.size());
	assert(ceres_vars_ori.size() == ceres_vars_bias_a.size());
	assert(ceres_vars_ori.size() == ceres_vars_pos.size());
	auto rT5 = boost::posix_time::microsec_clock::local_time();

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	std::cout << "[initialize-dynamic]: " << (int)summary.iterations.size() << " iterations | " << map_states.size() << " states, "
		<< map_features.size() << " feats (" << count_valid_features << " valid) | " << summary.num_parameters << " param and "
		<< summary.num_residuals << " res | cost " << std::fixed << summary.initial_cost << " => " << summary.final_cost << std::endl;

	auto rT6 = boost::posix_time::microsec_clock::local_time();

	timestamp = newest_cam_time;
	if (initializer_options.init_dyn_mle_max_iter != 0 && summary.termination_type != ceres::CONVERGENCE)
	{
		std::cout << "[initialize-dynamic]: Optimization failed: " << summary.message << std::endl;
		freeStateMemory();
		return false;
	}
	std::cout << "[initialize-dynamic]: " << summary.message << std::endl;

	auto getPose = [&](double timestamp)
	{
		Eigen::VectorXd state_imu = Eigen::VectorXd::Zero(16);

		for (int i = 0; i < 4; i++)
			state_imu(0 + i) = ceres_vars_ori[map_states[timestamp]][i];

		for (int i = 0; i < 3; i++)
		{
			state_imu(4 + i) = ceres_vars_pos[map_states[timestamp]][i];
			state_imu(7 + i) = ceres_vars_vel[map_states[timestamp]][i];
			state_imu(10 + i) = ceres_vars_bias_g[map_states[timestamp]][i];
			state_imu(13 + i) = ceres_vars_bias_a[map_states[timestamp]][i];
		}

		return state_imu;
	};

	assert(map_states.find(newest_cam_time) != map_states.end());

	if (imu_state_ == nullptr)
		imu_state_ = std::make_shared<imuState>();

	Eigen::VectorXd imu_state = getPose(newest_cam_time);
	imu_state_->setValue(imu_state);
	imu_state_->setFej(imu_state);

	for (auto const &state_pair : map_states)
	{
		Eigen::VectorXd pose = getPose(state_pair.first);

		if (clones_imu_.find(state_pair.first) == clones_imu_.end())
		{
			auto pose_ = std::make_shared<poseJpl>();
			pose_->setValue(pose.block(0, 0, 7, 1));
			pose_->setFej(pose.block(0, 0, 7, 1));
			clones_imu_.insert({state_pair.first, pose_});
		}
		else
		{
			clones_imu_.at(state_pair.first)->setValue(pose.block(0, 0, 7, 1));
			clones_imu_.at(state_pair.first)->setFej(pose.block(0, 0, 7, 1));
		}
	}

	for (auto const &feat_pair : map_features)
	{
		Eigen::Vector3d feature;
		feature << ceres_vars_feat[feat_pair.second][0], ceres_vars_feat[feat_pair.second][1], ceres_vars_feat[feat_pair.second][2];

		if (map_points_.find(feat_pair.first) == map_points_.end())
		{
			auto feature_ = std::make_shared<mapPoint>();
			feature_->feature_id = feat_pair.first;
			feature_->setPointXYZ(feature, false);
			feature_->setPointXYZ(feature, true);
			map_points_.insert({feat_pair.first, feature_});
		}
		else
		{
			map_points_.at(feat_pair.first)->feature_id = feat_pair.first;
			map_points_.at(feat_pair.first)->setPointXYZ(feature, false);
			map_points_.at(feat_pair.first)->setPointXYZ(feature, true);
		}
	}

	std::vector<std::pair<const double *, const double *>> covariance_blocks;
	int state_index = map_states[newest_cam_time];

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_ori[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index]));

	ceres::Covariance::Options options_cov;
	options_cov.null_space_rank = (!initializer_options.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8));
	options_cov.min_reciprocal_condition_number = initializer_options.init_dyn_min_rec_cond;

	options_cov.apply_loss_function = true;
	options_cov.num_threads = initializer_options.init_dyn_mle_max_threads;

	ceres::Covariance problem_cov(options_cov);
	bool success = problem_cov.Compute(covariance_blocks, &problem);

	if (!success)
	{
		std::cout << "[initialize-dynamic]: Covariance recovery failed..." << std::endl;
		freeStateMemory();
		return false;
	}

	order.clear();
	order.push_back(imu_state_);
	covariance = Eigen::MatrixXd::Zero(imu_state_->getSize(), imu_state_->getSize());
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> cov_tmp = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], cov_tmp.data()));
	covariance.block(0, 0, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(3, 3, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(6, 6, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(9, 9, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(12, 12, 3, 3) = cov_tmp.eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(0, 3, 3, 3) = cov_tmp.eval();
	covariance.block(3, 0, 3, 3) = cov_tmp.transpose();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(0, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(0, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(0, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 0, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(3, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(3, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(3, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 3, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(6, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 6, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(6, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 6, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(9, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 9, 3, 3) = cov_tmp.transpose().eval();

	covariance.block(0, 0, 3, 3) *= initializer_options.init_dyn_inflation_orientation;
	covariance.block(6, 6, 3, 3) *= initializer_options.init_dyn_inflation_velocity;
	covariance.block(9, 9, 3, 3) *= initializer_options.init_dyn_inflation_bias_gyro;
	covariance.block(12, 12, 3, 3) *= initializer_options.init_dyn_inflation_bias_accel;

	covariance = 0.5 * (covariance + covariance.transpose());
	Eigen::Vector3d sigmas_vel = covariance.block(6, 6, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_bg = covariance.block(9, 9, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_ba = covariance.block(12, 12, 3, 3).diagonal().transpose().cwiseSqrt();
	std::cout << "[initialize-dynamic]: vel priors = " << sigmas_vel(0) << ", " << sigmas_vel(1) << ", " << sigmas_vel(2) << std::endl;
	std::cout << "[initialize-dynamic]: bg priors = " << sigmas_bg(0) << ", " << sigmas_bg(1) << ", " << sigmas_bg(2) << std::endl;
	std::cout << "[initialize-dynamic]: ba priors = " << sigmas_ba(0) << ", " << sigmas_ba(1) << ", " << sigmas_ba(2) << std::endl;

	Eigen::MatrixXd x = imu_state_->value();
	x.block(4, 0, 3, 1).setZero();
	imu_state_->setValue(x);
	imu_state_->setFej(x);

	auto rT7 = boost::posix_time::microsec_clock::local_time();
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " sec for prelim tests" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " sec for linsys setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT4 - rT3).total_microseconds() * 1e-6 << " sec for linsys" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT5 - rT4).total_microseconds() * 1e-6 << " sec for ceres opt setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT6 - rT5).total_microseconds() * 1e-6 << " sec for ceres opt" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT6).total_microseconds() * 1e-6 << " sec for ceres covariance" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT1).total_microseconds() * 1e-6 << " sec total for initialization" << std::endl;
	freeStateMemory();

	return true;
}
  private:
    inertialInitializerOptions initializer_options;
    std::shared_ptr<featureDatabase> db;
    std::shared_ptr<std::vector<imuData>> imu_data;
  public:
    explicit dynamicInitializer(const inertialInitializerOptions &initializer_options_, std::shared_ptr<featureDatabase> db_,
    std::shared_ptr<std::vector<imuData>> imu_data_) {

}
    bool initialize(double &timestamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<baseType>> &order,
    std::shared_ptr<imuState> &imu_state_, std::map<double, std::shared_ptr<poseJpl>> &clones_imu_,
    std::unordered_map<size_t, std::shared_ptr<mapPoint>> &map_points_) {
	auto rT1 = boost::posix_time::microsec_clock::local_time();
	double newest_cam_time = -1;
	for (auto const &feat : db->getInternalData())
	{
		for (auto const &cam_time_pair : feat.second->timestamps)
		{
			for (auto const &time : cam_time_pair.second)
			{
				newest_cam_time = std::max(newest_cam_time, time);
			}
		}
	}

	double oldest_time = newest_cam_time - initializer_options.init_window_time;

	if (newest_cam_time < 0 || oldest_time < 0) return false;

	db->cleanUpOldmeasurements(oldest_time);

	bool have_old_imu_readings = false;
	auto it_imu = imu_data->begin();

	while (it_imu != imu_data->end() && it_imu->timestamp < oldest_time + initializer_options.calib_camimu_dt)
	{
		have_old_imu_readings = true;
		it_imu = imu_data->erase(it_imu);
	}

	if (db->getInternalData().size() < 0.75 * initializer_options.init_max_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Only " << db->getInternalData().size() << " valid features of required ("
			<< 0.95 * initializer_options.init_max_features << " thresh)" << std::endl;
		return false;
	}

	if (imu_data->size() < 2 || !have_old_imu_readings)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Waiting for window to reach full size (" << imu_data->size() << " imu readings)" << std::endl;
		return false;
	}

	std::unordered_map<size_t, std::shared_ptr<feature>> features;

	for (const auto &feat : db->getInternalData())
	{
		auto feat_new = std::make_shared<feature>();
		feat_new->feature_id = feat.second->feature_id;
		feat_new->uvs = feat.second->uvs;
		feat_new->uvs_norm = feat.second->uvs_norm;
		feat_new->timestamps = feat.second->timestamps;
		features.insert({feat.first, feat_new});
	}

	const int min_num_meas_to_optimize = (int)initializer_options.init_window_time;
	const int min_valid_features = 8;

	bool have_stereo = false;
	int count_valid_features = 0;
	std::map<size_t, int> map_features_num_meas;
	int num_measurements = 0;
	double oldest_camera_time = INFINITY;
	std::map<double, bool> map_camera_times;
	map_camera_times[newest_cam_time] = true;
	std::map<size_t, bool> map_camera_ids;
	double pose_dt_avg = initializer_options.init_window_time / (double)(initializer_options.init_dyn_num_pose + 1);

	for (auto const &feat : features)
	{
		std::vector<double> times;
		std::map<size_t, bool> cam_ids;

		for (auto const &cam_time : feat.second->timestamps)
		{
			for (double time : cam_time.second)
			{
				double time_dt = INFINITY;

				for (auto const &tmp : map_camera_times)
					time_dt = std::min(time_dt, std::abs(time - tmp.first));

				for (auto const &tmp : times)
					time_dt = std::min(time_dt, std::abs(time - tmp));

				if (time_dt >= pose_dt_avg || time_dt == 0.0)
				{
					times.push_back(time);
					cam_ids[cam_time.first] = true;
				}
			}
		}

		map_features_num_meas[feat.first] = (int)times.size();
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		for (auto const &tmp : times)
		{
			map_camera_times[tmp] = true;
			oldest_camera_time = std::min(oldest_camera_time, tmp);
			num_measurements += 2;
		}

		for (auto const &tmp : cam_ids)
			map_camera_ids[tmp.first] = true;

		if (cam_ids.size() > 1) have_stereo = true;

		count_valid_features++;
	}

	if ((int)map_camera_times.size() < initializer_options.init_dyn_num_pose)
		return false;

	if (count_valid_features < min_valid_features)
	{
		std::cout << "[initialize-dynamic]: Only " << count_valid_features << " valid features of required " << min_valid_features << std::endl;
		return false;
	}

	Eigen::Vector3d gyroscope_bias = initializer_options.init_dyn_bias_g;
	Eigen::Vector3d accelerometer_bias = initializer_options.init_dyn_bias_a;

	double accel_inI_norm = 0.0;
	double theta_inI_norm = 0.0;
	double time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
	double time_1_in_imu = newest_cam_time + initializer_options.calib_camimu_dt;
	std::vector<imuData> readings = initializerHelper::selectImuReadings(*imu_data, time_0_in_imu, time_1_in_imu);

	assert(readings.size() > 2);
	for (size_t k = 0; k < readings.size() - 1; k++)
	{
		auto imu_0 = readings.at(k);
		auto imu_1 = readings.at(k + 1);
		double dt = imu_1.timestamp - imu_0.timestamp;
		Eigen::Vector3d gyr = 0.5 * (imu_0.gyr + imu_1.gyr) - gyroscope_bias;
		Eigen::Vector3d acc = 0.5 * (imu_0.acc + imu_1.acc) - accelerometer_bias;
		theta_inI_norm += ( -gyr * dt).norm();
		accel_inI_norm += acc.norm();
	}

	accel_inI_norm /= (double)(readings.size() - 1);

	if (180.0 / M_PI * theta_inI_norm < initializer_options.init_dyn_min_deg)
	{
		std::cout << "[initialize-dynamic]: Gyroscope only " << 180.0 / M_PI * theta_inI_norm << " degree change ("
			<< initializer_options.init_dyn_min_deg << " thresh)" << std::endl;
		return false;
	}
	std::cout << std::fixed << "[initialize-dynamic]: |theta_I| = " << 180.0 / M_PI * theta_inI_norm
		<< " deg and |accel| = " << accel_inI_norm << std::endl;

 	auto rT2 = boost::posix_time::microsec_clock::local_time();

	const bool use_single_depth = false;
	int size_feature = (use_single_depth) ? 1 : 3;
	int num_features = count_valid_features;
	int system_size = size_feature * num_features + 3 + 3;

	if (num_measurements < system_size)
	{
		std::cout << "[initialize-dynamic]: Not enough feature measurements (" << num_measurements << " meas vs " << system_size
			<< " state size)" << std::endl;

		return false;
	}

	assert(oldest_camera_time < newest_cam_time);
	double last_camera_timestamp = 0.0;
	std::map<double, std::shared_ptr<preIntegrationV1>> map_camera_pre_integration_I0toIi, map_camera_pre_integration_IitoIi1; //map_camera_cpi_I0toIi, map_camera_cpi_IitoIi1;
	for (auto const &time_pair : map_camera_times)
	{
		double current_time = time_pair.first;

		if (current_time == oldest_camera_time)
		{
			map_camera_pre_integration_I0toIi.insert({current_time, nullptr});
			map_camera_pre_integration_IitoIi1.insert({current_time, nullptr});
			last_camera_timestamp = current_time;
			continue;
		}

		double pre_integration_I0toIi1_time_0_in_imu = oldest_camera_time + initializer_options.calib_camimu_dt;
		double pre_integration_I0toIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_I0toIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb, initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_I0toIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_I0toIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_I0toIi1_time_0_in_imu, pre_integration_I0toIi1_time_1_in_imu);

		if (pre_integration_I0toIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)
				<< " in has " << pre_integration_I0toIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_I0toIi1_dt_imu = pre_integration_I0toIi1_readings.at(pre_integration_I0toIi1_readings.size() - 1).timestamp - pre_integration_I0toIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_I0toIi1_dt_imu - (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_I0toIi1_dt_imu << " of "
				<< (pre_integration_I0toIi1_time_1_in_imu - pre_integration_I0toIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_I0toIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_I0toIi1_readings.at(k);
			auto imu_1 = pre_integration_I0toIi1_readings.at(k + 1);
			pre_integration_I0toIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		double pre_integration_IitoIi1_time_0_in_imu = last_camera_timestamp + initializer_options.calib_camimu_dt;
		double pre_integration_IitoIi1_time_1_in_imu = current_time + initializer_options.calib_camimu_dt;
		auto pre_integration_IitoIi1 = std::make_shared<preIntegrationV1>(initializer_options.sigma_w, initializer_options.sigma_wb,
			initializer_options.sigma_a, initializer_options.sigma_ab, true);
		pre_integration_IitoIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);

		std::vector<imuData> pre_integration_IitoIi1_readings =
			initializerHelper::selectImuReadings(*imu_data, pre_integration_IitoIi1_time_0_in_imu, pre_integration_IitoIi1_time_1_in_imu);

		if (pre_integration_IitoIi1_readings.size() < 2)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera " << (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << " in has "
				<< pre_integration_IitoIi1_readings.size() << " IMU readings" << std::endl;
			return false;
		}

		double pre_integration_IitoIi1_dt_imu = pre_integration_IitoIi1_readings.at(pre_integration_IitoIi1_readings.size() - 1).timestamp - pre_integration_IitoIi1_readings.at(0).timestamp;

		if (std::abs(pre_integration_IitoIi1_dt_imu - (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu)) > 0.01)
		{
			std::cout << std::fixed << "[initialize-dynamic]: Camera IMU was only propagated " << pre_integration_IitoIi1_dt_imu << " of "
				<< (pre_integration_IitoIi1_time_1_in_imu - pre_integration_IitoIi1_time_0_in_imu) << std::endl;
			return false;
		}

		for (size_t k = 0; k < pre_integration_IitoIi1_readings.size() - 1; k++)
		{
			auto imu_0 = pre_integration_IitoIi1_readings.at(k);
			auto imu_1 = pre_integration_IitoIi1_readings.at(k + 1);
			pre_integration_IitoIi1->feedImu(imu_0.timestamp, imu_1.timestamp, imu_0.gyr, imu_0.acc, imu_1.gyr, imu_1.acc);
		}

		map_camera_pre_integration_I0toIi.insert({current_time, pre_integration_I0toIi1});
		map_camera_pre_integration_IitoIi1.insert({current_time, pre_integration_IitoIi1});
		last_camera_timestamp = current_time;
	}

	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_measurements, system_size);
	Eigen::VectorXd b = Eigen::VectorXd::Zero(num_measurements);
	std::cout << "[initialize-dynamic]: System of " << num_measurements << " measurement x " << system_size << " states created (" << num_features
		<< " features, " << ((have_stereo) ? "stereo" : "mono") << ")" << std::endl;

	int index_meas = 0;
	int idx_feat = 0;
	std::map<size_t, int> A_index_features;

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (A_index_features.find(feat.first) == A_index_features.end())
		{
			A_index_features.insert({feat.first, idx_feat});
			idx_feat += 1;
		}

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);

				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_norm;
				uv_norm << (double)feat.second->uvs_norm.at(cam_id).at(i)(0), (double)feat.second->uvs_norm.at(cam_id).at(i)(1);

				double dt = 0.0;
				Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
				Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
				if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
				{
					dt = map_camera_pre_integration_I0toIi.at(time)->dt;
					R_I0toIk = map_camera_pre_integration_I0toIi.at(time)->R_tau_k;
					alpha_I0toIk = map_camera_pre_integration_I0toIi.at(time)->alpha_tau;
				}

				Eigen::MatrixXd H_proj = Eigen::MatrixXd::Zero(2, 3);
				H_proj << 1, 0, -uv_norm(0), 0, 1, -uv_norm(1);
				Eigen::MatrixXd Y = H_proj * R_ItoC * R_I0toIk;
				Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(2, system_size);
				Eigen::MatrixXd b_i = Y * alpha_I0toIk - H_proj * p_IinC;

				if (size_feature == 1)
				{
					assert(false);
				}
				else
				{
					H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y;
				}

				H_i.block(0, size_feature * num_features + 0, 2, 3) = -dt * Y;
				H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * dt * dt * Y;

				A.block(index_meas, 0, 2, A.cols()) = H_i;
				b.block(index_meas, 0, 2, 1) = b_i;
				index_meas += 2;
			}
		}
	}
	auto rT3 = boost::posix_time::microsec_clock::local_time();

	Eigen::MatrixXd A1 = A.block(0, 0, A.rows(), A.cols() - 3);
	Eigen::MatrixXd A1A1_inv = (A1.transpose() * A1).llt().solve(Eigen::MatrixXd::Identity(A1.cols(), A1.cols()));
	Eigen::MatrixXd A2 = A.block(0, A.cols() - 3, A.rows(), 3);
	Eigen::MatrixXd Temp = A2.transpose() * (Eigen::MatrixXd::Identity(A1.rows(), A1.rows()) - A1 * A1A1_inv * A1.transpose());
	Eigen::MatrixXd D = Temp * A2;
	Eigen::MatrixXd d = Temp * b;
	Eigen::Matrix<double, 7, 1> coeff = initializerHelper::computeDongsiCoeff(D, d, initializer_options.gravity_mag);

	assert(coeff(0) == 1);
	Eigen::Matrix<double, 6, 6> companion_matrix = Eigen::Matrix<double, 6, 6>::Zero(coeff.rows() - 1, coeff.rows() - 1);
	companion_matrix.diagonal(-1).setOnes();
	companion_matrix.col(companion_matrix.cols() - 1) = - coeff.reverse().head(coeff.rows() - 1);
	Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> svd_0(companion_matrix);
	Eigen::MatrixXd singular_values_0 = svd_0.singularValues();
	double cond_0 = singular_values_0(0) / singular_values_0(singular_values_0.rows() - 1);

	std::cout << std::fixed << "[initialize-dynamic]: CM cond = " << cond_0 << " | rank = " << (int)svd_0.rank() << " of " << (int)companion_matrix.cols()
		<< " (" << svd_0.threshold() << " thresh)" << std::endl;

	if (svd_0.rank() != companion_matrix.rows())
	{
		std::cout << "[initialize-dynamic]: Eigenvalue decomposition not full rank" << std::endl;
		return false;
	}

	Eigen::EigenSolver<Eigen::Matrix<double, 6, 6>> solver(companion_matrix, false);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "[initialize-dynamic]: failed to compute the eigenvalue decomposition" << std::endl;
		return false;
	}

	bool lambda_found = false;
	double lambda_min = -1;
	double cost_min = INFINITY;
	Eigen::MatrixXd I_dd = Eigen::MatrixXd::Identity(D.rows(), D.rows());

	for (int i = 0; i < solver.eigenvalues().size(); i++) {
		auto val = solver.eigenvalues()(i);
		if (val.imag() == 0)
		{
			double lambda = val.real();
			Eigen::MatrixXd D_lambdaI_inv = (D - lambda * I_dd).llt().solve(I_dd);
			Eigen::VectorXd state_grav = D_lambdaI_inv * d;
			double cost = std::abs(state_grav.norm() - initializer_options.gravity_mag);

			if (!lambda_found || cost < cost_min)
			{
				lambda_found = true;
				lambda_min = lambda;
				cost_min = cost;
			}
		}
	}

	if (!lambda_found)
	{
		std::cout << "[initialize-dynamic]: Failed to find a real eigenvalue" << std::endl;
		return false;
	}

	std::cout << std::fixed << "[initialize-dynamic]: Smallest real eigenvalue = " << lambda_min << " (cost of "
		<< cost_min << ")" << std::endl;

	Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).llt().solve(I_dd);
	Eigen::VectorXd state_grav = D_lambdaI_inv * d;

	Eigen::VectorXd state_feat_vel = - A1A1_inv * A1.transpose() * A2 * state_grav + A1A1_inv * A1.transpose() * b;
	Eigen::MatrixXd x_hat = Eigen::MatrixXd::Zero(system_size, 1);
	x_hat.block(0, 0, size_feature * num_features + 3, 1) = state_feat_vel;
	x_hat.block(size_feature * num_features + 3, 0, 3, 1) = state_grav;
	Eigen::Vector3d v_I0inI0 = x_hat.block(size_feature * num_features + 0, 0, 3, 1);
	std::cout << std::fixed << "[initialize-dynamic]: Velocity in I0 was " << v_I0inI0(0) << ", " << v_I0inI0(1) << ", " << v_I0inI0(2)
		<< " and |v| = " << v_I0inI0.norm() << std::endl;

	Eigen::Vector3d gravity_inI0 = x_hat.block(size_feature * num_features + 3, 0, 3, 1);
	double init_max_grav_difference = 1e-3;

	if (std::abs(gravity_inI0.norm() - initializer_options.gravity_mag) > init_max_grav_difference)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Gravity did not converge (" << std::abs(gravity_inI0.norm() - initializer_options.gravity_mag)
			<< " > " << init_max_grav_difference << ")" << std::endl;
		return false;
	}

	std::cout << "[initialize-dynamic]: Gravity in I0 was " << gravity_inI0(0) << ", " << gravity_inI0(1) << ", " << gravity_inI0(2) << " and |g| = " << gravity_inI0.norm() << std::endl;
	auto rT4 = boost::posix_time::microsec_clock::local_time();

	std::map<double, Eigen::VectorXd> ori_I0toIi, pos_IiinI0, vel_IiinI0;

	for (auto const &time_pair : map_camera_times)
	{
		double time = time_pair.first;

		double dt = 0.0;
		Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
		Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
		Eigen::MatrixXd beta_I0toIk = Eigen::MatrixXd::Zero(3, 1);

		if (map_camera_pre_integration_I0toIi.find(time) != map_camera_pre_integration_I0toIi.end() && map_camera_pre_integration_I0toIi.at(time) != nullptr)
		{
			auto pre_integration = map_camera_pre_integration_I0toIi.at(time);
			dt = pre_integration->dt;
			R_I0toIk = pre_integration->R_tau_k;
			alpha_I0toIk = pre_integration->alpha_tau;
			beta_I0toIk = pre_integration->beta_tau;
		}

		Eigen::Vector3d p_IkinI0 = v_I0inI0 * dt - 0.5 * gravity_inI0 * dt * dt + alpha_I0toIk;
		Eigen::Vector3d v_IkinI0 = v_I0inI0 - gravity_inI0 * dt + beta_I0toIk;

		ori_I0toIi.insert({time, quatType::rotToQuat(R_I0toIk)});
		pos_IiinI0.insert({time, p_IkinI0});
		vel_IiinI0.insert({time, v_IkinI0});
	}

	count_valid_features = 0;
	std::map<size_t, Eigen::Vector3d> features_inI0;
	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		Eigen::Vector3d p_FinI0;
		if (size_feature == 1)
			assert(false);
		else
			p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first), 0, 3, 1);

		bool is_behind = false;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t cam_id = cam_time.first;
			Eigen::Vector4d q_ItoC = initializer_options.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
			Eigen::Vector3d p_IinC = initializer_options.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
			Eigen::Vector3d p_FinC0 = quatType::quatToRot(q_ItoC) * p_FinI0 + p_IinC;

			if (p_FinC0(2) < 0) is_behind = true;
		}

		if (!is_behind)
		{
			features_inI0.insert({feat.first, p_FinI0});
			count_valid_features++;
		}
	}

	if (count_valid_features < min_valid_features)
	{
		std::cout << std::fixed << "[initialize-dynamic]: Not enough features for our mle (" << count_valid_features << " < "
			<< min_valid_features << ")" << std::endl;
		return false;
	}

	Eigen::Matrix3d R_GtoI0;
	initializerHelper::gramSchmidt(gravity_inI0, R_GtoI0);
	Eigen::Vector4d q_GtoI0 = quatType::rotToQuat(R_GtoI0);
	Eigen::Vector3d gravity;
	gravity << 0.0, 0.0, initializer_options.gravity_mag;
	std::map<double, Eigen::VectorXd> ori_GtoIi, pos_IiinG, vel_IiinG;
	std::map<size_t, Eigen::Vector3d> features_inG;

	for (auto const &time_pair : map_camera_times)
	{
		ori_GtoIi[time_pair.first] = quatType::quatMultiply(ori_I0toIi.at(time_pair.first), q_GtoI0);
		pos_IiinG[time_pair.first] = R_GtoI0.transpose() * pos_IiinI0.at(time_pair.first);
		vel_IiinG[time_pair.first] = R_GtoI0.transpose() * vel_IiinI0.at(time_pair.first);
	}

	for (auto const &feat : features_inI0)
	features_inG[feat.first] = R_GtoI0.transpose() * feat.second;

	ceres::Problem problem;

	std::map<double, int> map_states;
	std::vector<double*> ceres_vars_ori;
	std::vector<double*> ceres_vars_pos;
	std::vector<double*> ceres_vars_vel;
	std::vector<double*> ceres_vars_bias_g;
	std::vector<double*> ceres_vars_bias_a;

	std::map<size_t, int> map_features;
	std::vector<double*> ceres_vars_feat;

	std::map<size_t, int> map_calib_cam2imu;
	std::vector<double*> ceres_vars_calib_cam2imu_ori;
	std::vector<double*> ceres_vars_calib_cam2imu_pos;

	std::map<size_t, int> map_calib_cam;
	std::vector<double*> ceres_vars_calib_cam_intrinsics;

	auto freeStateMemory = [&]()
	{
		for (auto const &ptr : ceres_vars_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_vel)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_g)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_bias_a)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_feat)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_ori)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam2imu_pos)
			delete[] ptr;
		for (auto const &ptr : ceres_vars_calib_cam_intrinsics)
			delete[] ptr;
	};

	ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_SCHUR;
	options.trust_region_strategy_type = ceres::DOGLEG;
	options.num_threads = initializer_options.init_dyn_mle_max_threads;
	options.max_solver_time_in_seconds = initializer_options.init_dyn_mle_max_time;
	options.max_num_iterations = initializer_options.init_dyn_mle_max_iter;
	options.function_tolerance = 1e-5;
	options.gradient_tolerance = 1e-4 * options.function_tolerance;

	double timestamp_k = -1;

	for (auto const &time_pair : map_camera_times)
	{
		double timestamp_k1 = time_pair.first;
		std::shared_ptr<preIntegrationV1> pre_integration = map_camera_pre_integration_IitoIi1.at(timestamp_k1);
		Eigen::Matrix<double, 16, 1> state_k1;
		state_k1.block(0, 0, 4, 1) = ori_GtoIi.at(timestamp_k1);
		state_k1.block(4, 0, 3, 1) = pos_IiinG.at(timestamp_k1);
		state_k1.block(7, 0, 3, 1) = vel_IiinG.at(timestamp_k1);
		state_k1.block(10, 0, 3, 1) = gyroscope_bias;
		state_k1.block(13, 0, 3, 1) = accelerometer_bias;

		auto *var_ori = new double[4];
		for (int j = 0; j < 4; j++)
			var_ori[j] = state_k1(0 + j, 0);

		auto *var_pos = new double[3];
		auto *var_vel = new double[3];
		auto *var_bias_g = new double[3];
		auto *var_bias_a = new double[3];

		for (int j = 0; j < 3; j++)
		{
			var_pos[j] = state_k1(4 + j, 0);
			var_vel[j] = state_k1(7 + j, 0);
			var_bias_g[j] = state_k1(10 + j, 0);
			var_bias_a[j] = state_k1(13 + j, 0);
		}

		auto ceres_jplquat = new stateJplQuatLocal();
		problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
		problem.AddParameterBlock(var_pos, 3);
		problem.AddParameterBlock(var_vel, 3);
		problem.AddParameterBlock(var_bias_g, 3);
		problem.AddParameterBlock(var_bias_a, 3);

		if (map_states.empty())
		{
			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(13, 1);

			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_ori[j];

			for (int j = 0; j < 3; j++)
			{
				x_lin(4 + j) = var_pos[j];
				x_lin(7 + j) = var_bias_g[j];
				x_lin(10 + j) = var_bias_a[j];
			}

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(10, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(10, 10);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-5, 2);
			prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(0.05, 2);
			prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(0.10, 2);

			std::vector<std::string> x_types;
			std::vector<double *> factor_params;
			factor_params.push_back(var_ori);
			x_types.emplace_back("quat_yaw");
			factor_params.push_back(var_pos);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_g);
			x_types.emplace_back("vec3");
			factor_params.push_back(var_bias_a);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);
		}

		map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
		ceres_vars_ori.push_back(var_ori);
		ceres_vars_pos.push_back(var_pos);
		ceres_vars_vel.push_back(var_vel);
		ceres_vars_bias_g.push_back(var_bias_g);
		ceres_vars_bias_a.push_back(var_bias_a);

		if (pre_integration != nullptr)
		{
			assert(timestamp_k != -1);
			std::vector<double *> factor_params;
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
			factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
			factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
			auto *factor_imu = new factorImuPreIntegrationV1(pre_integration->dt, gravity, pre_integration->alpha_tau, pre_integration->beta_tau,
				pre_integration->q_tau_k, pre_integration->b_a_lin, pre_integration->b_w_lin, pre_integration->J_q, pre_integration->J_b,
				pre_integration->J_a, pre_integration->H_b, pre_integration->H_a, pre_integration->P_meas);

			problem.AddResidualBlock(factor_imu, nullptr, factor_params);
		}

		timestamp_k = timestamp_k1;
	}

	for (auto const &id_pair : map_camera_ids)
	{
		size_t cam_id = id_pair.first;
		if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end())
		{
			auto *var_calib_ori = new double[4];
			for (int j = 0; j < 4; j++)
				var_calib_ori[j] = initializer_options.camera_extrinsics.at(cam_id)(0 + j, 0);

			auto *var_calib_pos = new double[3];
			for (int j = 0; j < 3; j++)
				var_calib_pos[j] = initializer_options.camera_extrinsics.at(cam_id)(4 + j, 0);

			auto ceres_calib_jplquat = new stateJplQuatLocal();
			problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
			problem.AddParameterBlock(var_calib_pos, 3);

			map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
			ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
			ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
			for (int j = 0; j < 4; j++)
				x_lin(0 + j) = var_calib_ori[j];

			for (int j = 0; j < 3; j++)
				x_lin(4 + j) = var_calib_pos[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
			prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
			prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_ori);
			x_types.emplace_back("quat");
			factor_params.push_back(var_calib_pos);
			x_types.emplace_back("vec3");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
			{
				problem.SetParameterBlockConstant(var_calib_ori);
				problem.SetParameterBlockConstant(var_calib_pos);
			}
		}

		if (map_calib_cam.find(cam_id) == map_calib_cam.end())
		{
			auto *var_calib_cam = new double[8];
			for (int j = 0; j < 8; j++)
				var_calib_cam[j] = initializer_options.camera_intrinsics.at(cam_id)->getValue()(j, 0);

			problem.AddParameterBlock(var_calib_cam, 8);
			map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
			ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);

			Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
			for (int j = 0; j < 8; j++)
				x_lin(0 + j) = var_calib_cam[j];

			Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
			Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
			prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
			prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);

			std::vector<std::string> x_types;
			std::vector<double*> factor_params;
			factor_params.push_back(var_calib_cam);
			x_types.emplace_back("vec8");

			auto *factor_prior = new factorGenericPrior(x_lin, x_types, prior_Info, prior_grad);
			problem.AddResidualBlock(factor_prior, nullptr, factor_params);

			if (!initializer_options.init_dyn_mle_opt_calib)
				problem.SetParameterBlockConstant(var_calib_cam);
		}
	}
	assert(map_calib_cam2imu.size() == map_calib_cam.size());

	for (auto const &feat : features)
	{
		if (map_features_num_meas[feat.first] < min_num_meas_to_optimize) continue;

		if (features_inG.find(feat.first) == features_inG.end()) continue;

		for (auto const &cam_time : feat.second->timestamps)
		{
			size_t feat_id = feat.first;
			size_t cam_id = cam_time.first;
			bool is_fisheye = (std::dynamic_pointer_cast<cameraEqui>(initializer_options.camera_intrinsics.at(cam_id)) != nullptr);

			for (size_t i = 0; i < cam_time.second.size(); i++)
			{
				double time = feat.second->timestamps.at(cam_id).at(i);
				if (map_camera_times.find(time) == map_camera_times.end()) continue;

				Eigen::Vector2d uv_raw = feat.second->uvs.at(cam_id).at(i).block(0, 0, 2, 1).cast<double>();

				if (map_features.find(feat_id) == map_features.end())
				{
					auto *var_feat = new double[3];
					for (int j = 0; j < 3; j++)
						var_feat[j] = features_inG.at(feat_id)(j);

					problem.AddParameterBlock(var_feat, 3);
					map_features.insert({feat_id, (int)ceres_vars_feat.size()});
					ceres_vars_feat.push_back(var_feat);
				}

				std::vector<double *> factor_params;
				factor_params.push_back(ceres_vars_ori.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_pos.at(map_states.at(time)));
				factor_params.push_back(ceres_vars_feat.at(map_features.at(feat_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
				factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
				auto *factor_pinhole = new factorImageReprojCalib(uv_raw, initializer_options.sigma_pix, is_fisheye);

				ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
				problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
			}
		}
	}
	assert(ceres_vars_ori.size() == ceres_vars_bias_g.size());
	assert(ceres_vars_ori.size() == ceres_vars_vel.size());
	assert(ceres_vars_ori.size() == ceres_vars_bias_a.size());
	assert(ceres_vars_ori.size() == ceres_vars_pos.size());
	auto rT5 = boost::posix_time::microsec_clock::local_time();

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	std::cout << "[initialize-dynamic]: " << (int)summary.iterations.size() << " iterations | " << map_states.size() << " states, "
		<< map_features.size() << " feats (" << count_valid_features << " valid) | " << summary.num_parameters << " param and "
		<< summary.num_residuals << " res | cost " << std::fixed << summary.initial_cost << " => " << summary.final_cost << std::endl;

	auto rT6 = boost::posix_time::microsec_clock::local_time();

	timestamp = newest_cam_time;
	if (initializer_options.init_dyn_mle_max_iter != 0 && summary.termination_type != ceres::CONVERGENCE)
	{
		std::cout << "[initialize-dynamic]: Optimization failed: " << summary.message << std::endl;
		freeStateMemory();
		return false;
	}
	std::cout << "[initialize-dynamic]: " << summary.message << std::endl;

	auto getPose = [&](double timestamp)
	{
		Eigen::VectorXd state_imu = Eigen::VectorXd::Zero(16);

		for (int i = 0; i < 4; i++)
			state_imu(0 + i) = ceres_vars_ori[map_states[timestamp]][i];

		for (int i = 0; i < 3; i++)
		{
			state_imu(4 + i) = ceres_vars_pos[map_states[timestamp]][i];
			state_imu(7 + i) = ceres_vars_vel[map_states[timestamp]][i];
			state_imu(10 + i) = ceres_vars_bias_g[map_states[timestamp]][i];
			state_imu(13 + i) = ceres_vars_bias_a[map_states[timestamp]][i];
		}

		return state_imu;
	};

	assert(map_states.find(newest_cam_time) != map_states.end());

	if (imu_state_ == nullptr)
		imu_state_ = std::make_shared<imuState>();

	Eigen::VectorXd imu_state = getPose(newest_cam_time);
	imu_state_->setValue(imu_state);
	imu_state_->setFej(imu_state);

	for (auto const &state_pair : map_states)
	{
		Eigen::VectorXd pose = getPose(state_pair.first);

		if (clones_imu_.find(state_pair.first) == clones_imu_.end())
		{
			auto pose_ = std::make_shared<poseJpl>();
			pose_->setValue(pose.block(0, 0, 7, 1));
			pose_->setFej(pose.block(0, 0, 7, 1));
			clones_imu_.insert({state_pair.first, pose_});
		}
		else
		{
			clones_imu_.at(state_pair.first)->setValue(pose.block(0, 0, 7, 1));
			clones_imu_.at(state_pair.first)->setFej(pose.block(0, 0, 7, 1));
		}
	}

	for (auto const &feat_pair : map_features)
	{
		Eigen::Vector3d feature;
		feature << ceres_vars_feat[feat_pair.second][0], ceres_vars_feat[feat_pair.second][1], ceres_vars_feat[feat_pair.second][2];

		if (map_points_.find(feat_pair.first) == map_points_.end())
		{
			auto feature_ = std::make_shared<mapPoint>();
			feature_->feature_id = feat_pair.first;
			feature_->setPointXYZ(feature, false);
			feature_->setPointXYZ(feature, true);
			map_points_.insert({feat_pair.first, feature_});
		}
		else
		{
			map_points_.at(feat_pair.first)->feature_id = feat_pair.first;
			map_points_.at(feat_pair.first)->setPointXYZ(feature, false);
			map_points_.at(feat_pair.first)->setPointXYZ(feature, true);
		}
	}

	std::vector<std::pair<const double *, const double *>> covariance_blocks;
	int state_index = map_states[newest_cam_time];

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_ori[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_pos[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_vel[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index]));
	covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index]));

	covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index]));

	ceres::Covariance::Options options_cov;
	options_cov.null_space_rank = (!initializer_options.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8));
	options_cov.min_reciprocal_condition_number = initializer_options.init_dyn_min_rec_cond;

	options_cov.apply_loss_function = true;
	options_cov.num_threads = initializer_options.init_dyn_mle_max_threads;

	ceres::Covariance problem_cov(options_cov);
	bool success = problem_cov.Compute(covariance_blocks, &problem);

	if (!success)
	{
		std::cout << "[initialize-dynamic]: Covariance recovery failed..." << std::endl;
		freeStateMemory();
		return false;
	}

	order.clear();
	order.push_back(imu_state_);
	covariance = Eigen::MatrixXd::Zero(imu_state_->getSize(), imu_state_->getSize());
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> cov_tmp = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], cov_tmp.data()));
	covariance.block(0, 0, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(3, 3, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(6, 6, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(9, 9, 3, 3) = cov_tmp.eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(12, 12, 3, 3) = cov_tmp.eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], cov_tmp.data()));
	covariance.block(0, 3, 3, 3) = cov_tmp.eval();
	covariance.block(3, 0, 3, 3) = cov_tmp.transpose();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(0, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(0, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 0, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(0, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 0, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], cov_tmp.data()));
	covariance.block(3, 6, 3, 3) = cov_tmp.eval();
	covariance.block(6, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(3, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 3, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(3, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 3, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], cov_tmp.data()));
	covariance.block(6, 9, 3, 3) = cov_tmp.eval();
	covariance.block(9, 6, 3, 3) = cov_tmp.transpose().eval();
	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(6, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 6, 3, 3) = cov_tmp.transpose().eval();

	CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], cov_tmp.data()));
	covariance.block(9, 12, 3, 3) = cov_tmp.eval();
	covariance.block(12, 9, 3, 3) = cov_tmp.transpose().eval();

	covariance.block(0, 0, 3, 3) *= initializer_options.init_dyn_inflation_orientation;
	covariance.block(6, 6, 3, 3) *= initializer_options.init_dyn_inflation_velocity;
	covariance.block(9, 9, 3, 3) *= initializer_options.init_dyn_inflation_bias_gyro;
	covariance.block(12, 12, 3, 3) *= initializer_options.init_dyn_inflation_bias_accel;

	covariance = 0.5 * (covariance + covariance.transpose());
	Eigen::Vector3d sigmas_vel = covariance.block(6, 6, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_bg = covariance.block(9, 9, 3, 3).diagonal().transpose().cwiseSqrt();
	Eigen::Vector3d sigmas_ba = covariance.block(12, 12, 3, 3).diagonal().transpose().cwiseSqrt();
	std::cout << "[initialize-dynamic]: vel priors = " << sigmas_vel(0) << ", " << sigmas_vel(1) << ", " << sigmas_vel(2) << std::endl;
	std::cout << "[initialize-dynamic]: bg priors = " << sigmas_bg(0) << ", " << sigmas_bg(1) << ", " << sigmas_bg(2) << std::endl;
	std::cout << "[initialize-dynamic]: ba priors = " << sigmas_ba(0) << ", " << sigmas_ba(1) << ", " << sigmas_ba(2) << std::endl;

	Eigen::MatrixXd x = imu_state_->value();
	x.block(4, 0, 3, 1).setZero();
	imu_state_->setValue(x);
	imu_state_->setFej(x);

	auto rT7 = boost::posix_time::microsec_clock::local_time();
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " sec for prelim tests" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " sec for linsys setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT4 - rT3).total_microseconds() * 1e-6 << " sec for linsys" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT5 - rT4).total_microseconds() * 1e-6 << " sec for ceres opt setup" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT6 - rT5).total_microseconds() * 1e-6 << " sec for ceres opt" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT6).total_microseconds() * 1e-6 << " sec for ceres covariance" << std::endl;
	std::cout << std::fixed << "[initialize-dynamic]: " << (rT7 - rT1).total_microseconds() * 1e-6 << " sec total for initialization" << std::endl;
	freeStateMemory();

	return true;
}
  private:
    inertialInitializerOptions initializer_options;
    std::shared_ptr<featureDatabase> db;
    std::shared_ptr<std::vector<imuData>> imu_data;
};

class propagator {
  public:
    propagator(noiseManager noises_, double gravity_mag_) {
	noises.sigma_w_2 = std::pow(noises.sigma_w, 2);
	noises.sigma_a_2 = std::pow(noises.sigma_a, 2);
	noises.sigma_wb_2 = std::pow(noises.sigma_wb, 2);
	noises.sigma_ab_2 = std::pow(noises.sigma_ab, 2);

	gravity << 0.0, 0.0, gravity_mag_;

	have_last_prop_time_offset = false;
	last_prop_time_offset = 0.0;
}
    void feedImu(const imuData &imu_data, double oldest_time = -1) {
	v_imu_data.emplace_back(imu_data);

	cleanOldImuMeasurements(oldest_time - 0.10);
}
    void cleanOldImuMeasurements(double oldest_time) {
	if (oldest_time < 0)
		return;
	auto it0 = v_imu_data.begin();
	while (it0 != v_imu_data.end())
	{
		if (it0->timestamp < oldest_time)
		{
			it0 = v_imu_data.erase(it0);
		}
		else
		{
			it0++;
		}
	}
}
    void invalidateCache() {
	cache_imu_valid = false;
}
    void resetTransitionMatrices(std::shared_ptr<state> state_ptr);
    static imuData interpolateData(const imuData &imu_1, const imuData &imu_2, double timestamp) {
	double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp);

	imuData data;
	data.timestamp = timestamp;
	data.acc = (1 - lambda) * imu_1.acc + lambda * imu_2.acc;
	data.gyr = (1 - lambda) * imu_1.gyr + lambda * imu_2.gyr;

	return data;
}
    static std::vector<imuData> selectImuReadings(const std::vector<imuData> &v_imu_data_, double time_0, double time_1, bool warn = true) {
	std::vector<imuData> prop_data;

	if (v_imu_data_.empty())
	{
		if (warn)
			std::cout << "propagator::selectImuReadings(): No IMU measurements. IMU-CAMERA are likely messed up" << std::endl;

		return prop_data;
	}

	for (size_t i = 0; i < v_imu_data_.size() - 1; i++)
	{
		if (v_imu_data_.at(i + 1).timestamp > time_0 && v_imu_data_.at(i).timestamp < time_0)
		{
			imuData data = propagator::interpolateData(v_imu_data_.at(i), v_imu_data_.at(i + 1), time_0);
			prop_data.push_back(data);

			continue;
		}

		if (v_imu_data_.at(i).timestamp >= time_0 && v_imu_data_.at(i + 1).timestamp <= time_1)
		{
			prop_data.push_back(v_imu_data_.at(i));
			continue;
		}

		if (v_imu_data_.at(i + 1).timestamp > time_1)
		{
			if (v_imu_data_.at(i).timestamp > time_1 && i == 0)
			{
				break;
			}
			else if (v_imu_data_.at(i).timestamp > time_1)
			{
				imuData data = propagator::interpolateData(v_imu_data_.at(i - 1), v_imu_data_.at(i), time_1);
				prop_data.push_back(data);
			}
			else
			{
				prop_data.push_back(v_imu_data_.at(i));
			}

			if (prop_data.at(prop_data.size() - 1).timestamp != time_1)
			{
				imuData data = propagator::interpolateData(v_imu_data_.at(i), v_imu_data_.at(i + 1), time_1);
				prop_data.push_back(data);
			}

			break;
		}
	}

	if (prop_data.empty())
	{
		if (warn)
			std::cout << "propagator::selectImuReadings(): No IMU measurements to propagate with (" << (int)prop_data.size() << " of 2). IMU-CAMERA are likely messed up" << std::endl;

		return prop_data;
	}

	if (prop_data.at(prop_data.size() - 1).timestamp != time_1)
	{
		if (warn)
			std::cout << std::fixed << "propagator::selectImuReadings(): Missing inertial measurements to propagate with (" << time_1 - v_imu_data_.at(v_imu_data_.size() - 1).timestamp
				<< " sec missing)" << std::endl;

		imuData data = propagator::interpolateData(v_imu_data_.at(v_imu_data_.size() - 2), v_imu_data_.at(v_imu_data_.size() - 1), time_1);
		prop_data.push_back(data);
	}

	for (size_t i = 0; i < prop_data.size() - 1; i++)
	{
		if (std::abs(prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp) < 1e-12)
		{
			if (warn)
				std::cout << "propagator::selectImuReadings(): Zero DT between IMU reading " << (int)i << " and " << (int)(i + 1) << ", removing it" << std::endl;

			prop_data.erase(prop_data.begin() + i);
			i--;
		}
	}

	if (prop_data.size() < 2)
	{
		if (warn)
			std::cout << "propagator::selectImuReadings(): No IMU measurements to propagate with (" << (int)prop_data.size() << " of 2). IMU-CAMERA are likely messed up" << std::endl;

		return prop_data;
	}

	return prop_data;
}
    void propagateAndClone(std::shared_ptr<state> state_ptr, double timestamp) {
	if (state_ptr->timestamp == timestamp)
	{
		std::cout << "propagator::propagateAndClone(): Propagation called again at same timestep at last update timestep!" << std::endl;
		std::exit(EXIT_FAILURE);
	}

	if (state_ptr->timestamp > timestamp)
	{
		std::cout << "propagator::propagateAndClone(): Propagation called trying to propagate backwards in time!" << std::endl;
		std::cout << std::fixed << "propagator::propagateAndClone(): desired propagation = " << timestamp - state_ptr->timestamp << std::endl;
		std::exit(EXIT_FAILURE);
	}

	if (!have_last_prop_time_offset)
	{
		last_prop_time_offset = state_ptr->calib_dt_imu_cam->value()(0);
		have_last_prop_time_offset = true;
	}

	double t_off_new = state_ptr->calib_dt_imu_cam->value()(0);

	double time_0 = state_ptr->timestamp + last_prop_time_offset;
	double time_1 = timestamp + t_off_new;
	std::vector<imuData> prop_data;
	{
		prop_data = propagator::selectImuReadings(v_imu_data, time_0, time_1);
	}

	Eigen::MatrixXd phi_sum = Eigen::MatrixXd::Identity(state_ptr->imuIntrinsicSize() + 15, state_ptr->imuIntrinsicSize() + 15);
	Eigen::MatrixXd Qd_sum = Eigen::MatrixXd::Zero(state_ptr->imuIntrinsicSize() + 15, state_ptr->imuIntrinsicSize() + 15);
	double dt_sum = 0;

	if (prop_data.size() > 1)
	{
		for (size_t i = 0; i < prop_data.size() - 1; i++)
		{
			Eigen::MatrixXd F, Qdi;
			predictAndCompute(state_ptr, prop_data.at(i), prop_data.at(i + 1), F, Qdi);

			phi_sum = F * phi_sum;
			Qd_sum = F * Qd_sum * F.transpose() + Qdi;
			Qd_sum = 0.5 * (Qd_sum + Qd_sum.transpose());
			dt_sum += prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp;
		}
	}
	assert(std::abs((time_1 - time_0) - dt_sum) < 1e-4);

	Eigen::Vector3d last_acc = Eigen::Vector3d::Zero();
	Eigen::Vector3d last_gyr = Eigen::Vector3d::Zero();
	if (!prop_data.empty())
	{
		Eigen::Matrix3d Dw = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_dw->value());
		Eigen::Matrix3d Da = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_da->value());
		Eigen::Matrix3d Tg = state::Tg(state_ptr->calib_imu_tg->value());
		last_acc = state_ptr->calib_imu_acc->getRot() * Da * (prop_data.at(prop_data.size() - 1).acc - state_ptr->imu_ptr->getBa());
		last_gyr = state_ptr->calib_imu_gyr->getRot() * Dw * (prop_data.at(prop_data.size() - 1).gyr - state_ptr->imu_ptr->getBg() - Tg * last_acc);
	}

	std::vector<std::shared_ptr<baseType>> phi_order;
	phi_order.push_back(state_ptr->imu_ptr);

	if (state_ptr->options.do_calib_imu_intrinsics)
	{
		phi_order.push_back(state_ptr->calib_imu_dw);
		phi_order.push_back(state_ptr->calib_imu_da);

		if (state_ptr->options.do_calib_imu_g_sensitivity)
		{
			phi_order.push_back(state_ptr->calib_imu_tg);
		}

		if (state_ptr->options.imu_model == ImuModel::KALIBR)
		{
			phi_order.push_back(state_ptr->calib_imu_gyr);
		}
		else
		{
			phi_order.push_back(state_ptr->calib_imu_acc);
		}
	}

	stateHelper::ekfPropagation(state_ptr, phi_order, phi_order, phi_sum, Qd_sum);

	state_ptr->timestamp = timestamp;
	last_prop_time_offset = t_off_new;

	stateHelper::augmentClone(state_ptr, last_gyr);
}
    void predictAndCompute(std::shared_ptr<state> state_ptr, const imuData &data_minus, const imuData &data_plus,
    Eigen::MatrixXd &F, Eigen::MatrixXd &Qd) {
	double dt = data_plus.timestamp - data_minus.timestamp;

	Eigen::Matrix3d Dw = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_dw->value());
	Eigen::Matrix3d Da = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_da->value());
	Eigen::Matrix3d Tg = state::Tg(state_ptr->calib_imu_tg->value());

	Eigen::Vector3d a_hat1 = data_minus.acc - state_ptr->imu_ptr->getBa();
	Eigen::Vector3d a_hat2 = data_plus.acc - state_ptr->imu_ptr->getBa();
	Eigen::Vector3d a_hat_avg = 0.5 * (a_hat1 + a_hat2);

	Eigen::Vector3d a_uncorrected = a_hat_avg;
	Eigen::Matrix3d R_imu_acc = state_ptr->calib_imu_acc->getRot();
	a_hat1 = R_imu_acc * Da * a_hat1;
	a_hat2 = R_imu_acc * Da * a_hat2;
	a_hat_avg = R_imu_acc * Da * a_hat_avg;

	Eigen::Vector3d w_hat1 = data_minus.gyr - state_ptr->imu_ptr->getBg() - Tg * a_hat1;
	Eigen::Vector3d w_hat2 = data_plus.gyr - state_ptr->imu_ptr->getBg() - Tg * a_hat2;
	Eigen::Vector3d w_hat_avg = 0.5 * (w_hat1 + w_hat2);

	Eigen::Vector3d w_uncorrected = w_hat_avg;
	Eigen::Matrix3d R_imu_gyr = state_ptr->calib_imu_gyr->getRot();
	w_hat1 = R_imu_gyr * Dw * w_hat1;
	w_hat2 = R_imu_gyr * Dw * w_hat2;
	w_hat_avg = R_imu_gyr * Dw * w_hat_avg;

	Eigen::Matrix<double, 3, 18> Xi_sum = Eigen::Matrix<double, 3, 18>::Zero(3, 18);
	computeXiSum(state_ptr, dt, w_hat_avg, a_hat_avg, Xi_sum);

	Eigen::Vector4d new_q;
	Eigen::Vector3d new_v, new_p;
	predictMeanRk4(state_ptr, dt, w_hat1, a_hat1, w_hat2, a_hat2, new_q, new_v, new_p);

	F = Eigen::MatrixXd::Zero(state_ptr->imuIntrinsicSize() + 15, state_ptr->imuIntrinsicSize() + 15);
	Eigen::MatrixXd G = Eigen::MatrixXd::Zero(state_ptr->imuIntrinsicSize() + 15, 12);
	computeCoefficientAnalytic(state_ptr, dt, w_hat_avg, a_hat_avg, w_uncorrected, a_uncorrected, new_q, new_v, new_p, Xi_sum, F, G);

	Eigen::Matrix<double, 12, 12> Qc = Eigen::Matrix<double, 12, 12>::Zero();
	Qc.block(0, 0, 3, 3) = std::pow(noises.sigma_w, 2) / dt * Eigen::Matrix3d::Identity();
	Qc.block(3, 3, 3, 3) = std::pow(noises.sigma_a, 2) / dt * Eigen::Matrix3d::Identity();
	Qc.block(6, 6, 3, 3) = std::pow(noises.sigma_wb, 2) / dt * Eigen::Matrix3d::Identity();
	Qc.block(9, 9, 3, 3) = std::pow(noises.sigma_ab, 2) / dt * Eigen::Matrix3d::Identity();

	Qd = Eigen::MatrixXd::Zero(state_ptr->imuIntrinsicSize() + 15, state_ptr->imuIntrinsicSize() + 15);
	Qd = G * Qc * G.transpose();
	Qd = 0.5 * (Qd + Qd.transpose());

	Eigen::Matrix<double, 16, 1> imu_x = state_ptr->imu_ptr->value();
	imu_x.block(0, 0, 4, 1) = new_q;
	imu_x.block(4, 0, 3, 1) = new_p;
	imu_x.block(7, 0, 3, 1) = new_v;
	state_ptr->imu_ptr->setValue(imu_x);
	state_ptr->imu_ptr->setFej(imu_x);
}
    static Eigen::MatrixXd computeDwH(std::shared_ptr<state> state_ptr, const Eigen::Vector3d &gyr_uncorrected) {
	Eigen::Vector3d e_1 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 0);
	Eigen::Vector3d e_2 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 1);
	Eigen::Vector3d e_3 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 2);

	double w_1 = gyr_uncorrected(0);
	double w_2 = gyr_uncorrected(1);
	double w_3 = gyr_uncorrected(2);
	assert(state_ptr->options.do_calib_imu_intrinsics);

	Eigen::MatrixXd H_Dw = Eigen::MatrixXd::Zero(3, 6);
	if (state_ptr->options.imu_model == ImuModel::KALIBR)
		H_Dw << w_1 * Eigen::MatrixXd::Identity(3, 3), w_2 * e_2, w_2 * e_3, w_3 * e_3;
	else
		H_Dw << w_1 * e_1, w_2 * e_1, w_2 * e_2, w_3 * Eigen::MatrixXd::Identity(3, 3);

	return H_Dw;
}
    static Eigen::MatrixXd computeDaH(std::shared_ptr<state> state_ptr, const Eigen::Vector3d &acc_uncorrected) {
	Eigen::Vector3d e_1 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 0);
	Eigen::Vector3d e_2 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 1);
	Eigen::Vector3d e_3 = Eigen::MatrixXd::Identity(3, 3).block<3, 1>(0, 2);

	double a_1 = acc_uncorrected(0);
	double a_2 = acc_uncorrected(1);
	double a_3 = acc_uncorrected(2);
	assert(state_ptr->options.do_calib_imu_intrinsics);

	Eigen::MatrixXd H_Da = Eigen::MatrixXd::Zero(3, 6);
	if (state_ptr->options.imu_model == ImuModel::KALIBR)
		H_Da << a_1 * Eigen::MatrixXd::Identity(3, 3), a_2 * e_2, a_2 * e_3, a_3 * e_3;
	else
		H_Da << a_1 * e_1, a_2 * e_1, a_2 * e_2, a_3 * Eigen::MatrixXd::Identity(3, 3);

	return H_Da;
}
    static Eigen::MatrixXd computeTgH(std::shared_ptr<state> state_ptr, const Eigen::Vector3d &acc) {
	double a_1 = acc(0);
	double a_2 = acc(1);
	double a_3 = acc(2);
	assert(state_ptr->options.do_calib_imu_intrinsics);
	assert(state_ptr->options.do_calib_imu_g_sensitivity);

	Eigen::MatrixXd H_Tg = Eigen::MatrixXd::Zero(3, 9);
	H_Tg << a_1 * Eigen::MatrixXd::Identity(3, 3), a_2 * Eigen::MatrixXd::Identity(3, 3), a_3 * Eigen::MatrixXd::Identity(3, 3);

	return H_Tg;
}
    bool have_last_prop_time_offset;
    double last_prop_time_offset;
  protected:
    void predictMeanRk4(std::shared_ptr<state> state_ptr, double dt, const Eigen::Vector3d &un_gyr_0, const Eigen::Vector3d &un_acc_0,
    const Eigen::Vector3d &un_gyr_1, const Eigen::Vector3d &un_acc_1, Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p) {
	Eigen::Vector3d un_gyr = un_gyr_0;
	Eigen::Vector3d un_acc = un_acc_0;
	Eigen::Vector3d w_alpha = (un_gyr_1 - un_gyr_0) / dt;
	Eigen::Vector3d a_jerk = (un_acc_1 - un_acc_0) / dt;

	Eigen::Vector4d q_0 = state_ptr->imu_ptr->getQuat();
	Eigen::Vector3d p_0 = state_ptr->imu_ptr->getPos();
	Eigen::Vector3d v_0 = state_ptr->imu_ptr->getVel();

	Eigen::Vector4d dq_0 = {0, 0, 0, 1};
	Eigen::Vector4d q_0_dot = 0.5 * quatType::omega(un_gyr) * dq_0;
	Eigen::Vector3d p_0_dot = v_0;
	Eigen::Matrix3d R_0_world = quatType::quatToRot(quatType::quatMultiply(dq_0, q_0));
	Eigen::Vector3d v_0_dot = R_0_world.transpose() * un_acc - gravity;

	Eigen::Vector4d k1_q = q_0_dot * dt;
	Eigen::Vector3d k1_p = p_0_dot * dt;
	Eigen::Vector3d k1_v = v_0_dot * dt;

	un_gyr += 0.5 * w_alpha * dt;
	un_acc += 0.5 * a_jerk * dt;

	Eigen::Vector4d dq_1 = quatType::quatNorm(dq_0 + 0.5 * k1_q);
	Eigen::Vector3d v_1 = v_0 + 0.5 * k1_v;
	Eigen::Vector4d q_1_dot = 0.5 * quatType::omega(un_gyr) * dq_1;
	Eigen::Vector3d p_1_dot = v_1;
	Eigen::Matrix3d R_1_world = quatType::quatToRot(quatType::quatMultiply(dq_1, q_0));
	Eigen::Vector3d v_1_dot = R_1_world.transpose() * un_acc - gravity;

	Eigen::Vector4d k2_q = q_1_dot * dt;
	Eigen::Vector3d k2_p = p_1_dot * dt;
	Eigen::Vector3d k2_v = v_1_dot * dt;

	Eigen::Vector4d dq_2 = quatType::quatNorm(dq_0 + 0.5 * k2_q);
	Eigen::Vector3d v_2 = v_0 + 0.5 * k2_v;

	Eigen::Vector4d q_2_dot = 0.5 * quatType::omega(un_gyr) * dq_2;
	Eigen::Vector3d p_2_dot = v_2;
	Eigen::Matrix3d R_2_world = quatType::quatToRot(quatType::quatMultiply(dq_2, q_0));
	Eigen::Vector3d v_2_dot = R_2_world.transpose() * un_acc - gravity;

	Eigen::Vector4d k3_q = q_2_dot * dt;
	Eigen::Vector3d k3_p = p_2_dot * dt;
	Eigen::Vector3d k3_v = v_2_dot * dt;

	un_gyr += 0.5 * w_alpha * dt;
	un_acc += 0.5 * a_jerk * dt;

	Eigen::Vector4d dq_3 = quatType::quatNorm(dq_0 + k3_q);
	Eigen::Vector3d v_3 = v_0 + k3_v;

	Eigen::Vector4d q_3_dot = 0.5 * quatType::omega(un_gyr) * dq_3;
	Eigen::Vector3d p_3_dot = v_3;
	Eigen::Matrix3d R_3_world = quatType::quatToRot(quatType::quatMultiply(dq_3, q_0));
	Eigen::Vector3d v_3_dot = R_3_world.transpose() * un_acc - gravity;

	Eigen::Vector4d k4_q = q_3_dot * dt;
	Eigen::Vector3d k4_p = p_3_dot * dt;
	Eigen::Vector3d k4_v = v_3_dot * dt;

	Eigen::Vector4d dq = quatType::quatNorm(dq_0 + (1.0 / 6.0) * k1_q + (1.0 / 3.0) * k2_q + (1.0 / 3.0) * k3_q + (1.0 / 6.0) * k4_q);
	new_q = quatType::quatMultiply(dq, q_0);
	new_p = p_0 + (1.0 / 6.0) * k1_p + (1.0 / 3.0) * k2_p + (1.0 / 3.0) * k3_p + (1.0 / 6.0) * k4_p;
	new_v = v_0 + (1.0 / 6.0) * k1_v + (1.0 / 3.0) * k2_v + (1.0 / 3.0) * k3_v + (1.0 / 6.0) * k4_v;
}
    void computeXiSum(std::shared_ptr<state> state_ptr, double dt, const Eigen::Vector3d &un_gyr,
    const Eigen::Vector3d &un_acc, Eigen::Matrix<double, 3, 18> &Xi_sum) {
	double un_gyr_norm = un_gyr.norm();
	double d_th = un_gyr_norm * dt;
	Eigen::Vector3d un_gyr_normalized = Eigen::Vector3d::Zero();

	if (un_gyr_norm > 1e-12)
		un_gyr_normalized = un_gyr / un_gyr_norm;

	double dt_2 = std::pow(dt, 2);
	double dt_3 = std::pow(dt, 3);
	double norm_2 = std::pow(un_gyr_norm, 2);
	double norm_3 = std::pow(un_gyr_norm, 3);
	double cos_dth = std::cos(d_th);
	double sin_dth = std::sin(d_th);
	double d_th_2 = std::pow(d_th, 2);
	double d_th_3 = std::pow(d_th, 3);

	Eigen::Matrix3d sK = quatType::skewSymmetric(un_gyr_normalized);
	Eigen::Matrix3d sK2 = sK * sK;
	Eigen::Matrix3d sA = quatType::skewSymmetric(un_acc);

	Eigen::Matrix3d R_temp = quatType::expSo3(-un_gyr * dt);
	Eigen::Matrix3d Jr_temp = quatType::JrighySo3(-un_gyr * dt);

	Eigen::Matrix3d Xi_1, Xi_2, Xi_3, Xi_4;

  	if (!(un_gyr_norm < 1.0 / 180 * M_PI / 2))
  	{
		Xi_1 = Eigen::Matrix3d::Identity() * dt + (1.0 - cos_dth) / un_gyr_norm * sK + (dt - sin_dth / un_gyr_norm) * sK2;

		Xi_2 = 1.0 / 2 * dt_2 * Eigen::Matrix3d::Identity() + (d_th - sin_dth) / norm_2 * sK + (1.0 / 2 * dt_2 - (1.0 - cos_dth) / norm_2) * sK2;

		Xi_3 = 1.0 / 2 * dt_2 * sA + (sin_dth - d_th) / norm_2 * sA * sK + (sin_dth - d_th * cos_dth) / norm_2 * sK * sA +
			(1.0 / 2 * dt_2 - (1.0 - cos_dth) / norm_2) * sA * sK2 +
			(1.0 / 2 * dt_2 + (1.0 - cos_dth - d_th * sin_dth) / norm_2) * (sK2 * sA + un_gyr_normalized.dot(un_acc) * sK) -
			(3 * sin_dth - 2 * d_th - d_th * cos_dth) / norm_2 * un_gyr_normalized.dot(un_acc) * sK2;

		Xi_4 = 1.0 / 6 * dt_3 * sA + (2 * (1.0 - cos_dth) - d_th_2) / (2 * norm_3) * sA * sK +
			((2 * (1.0 - cos_dth) - d_th * sin_dth) / norm_3) * sK * sA + ((sin_dth - d_th) / norm_3 + dt_3 / 6) * sA * sK2 +
			((d_th - 2 * sin_dth + 1.0 / 6 * d_th_3 + d_th * cos_dth) / norm_3) * (sK2 * sA + un_gyr_normalized.dot(un_acc) * sK) +
			(4 * cos_dth - 4 + d_th_2 + d_th * sin_dth) / norm_3 * un_gyr_normalized.dot(un_acc) * sK2;

	}
	else
	{
		Xi_1 = dt * (Eigen::Matrix3d::Identity() + sin_dth * sK + (1.0 - cos_dth) * sK2);

		Xi_2 = 1.0 / 2 * dt * Xi_1;

		Xi_3 = 1.0 / 2 * dt_2 * (sA + sin_dth * (-sA * sK + sK * sA + un_gyr_normalized.dot(un_acc) * sK2)
			+ (1.0 - cos_dth) * (sA * sK2 + sK2 * sA + un_gyr_normalized.dot(un_acc) * sK));

		Xi_4 = 1.0 / 3 * dt * Xi_3;
	}

	Xi_sum.setZero();
	Xi_sum.block<3, 3>(0, 0) = R_temp;
	Xi_sum.block<3, 3>(0, 3) = Xi_1;
	Xi_sum.block<3, 3>(0, 6) = Xi_2;
	Xi_sum.block<3, 3>(0, 9) = Jr_temp;
	Xi_sum.block<3, 3>(0, 12) = Xi_3;
	Xi_sum.block<3, 3>(0, 15) = Xi_4;
}
    void computeCoefficientAnalytic(std::shared_ptr<state> state_ptr, double dt, const Eigen::Vector3d &un_gyr, const Eigen::Vector3d &un_acc,
    const Eigen::Vector3d &gyr_uncorrected, const Eigen::Vector3d &acc_uncorrected, const Eigen::Vector4d &new_q, const Eigen::Vector3d &new_v,
    const Eigen::Vector3d &new_p, const Eigen::Matrix<double, 3, 18> &Xi_sum, Eigen::MatrixXd &F, Eigen::MatrixXd &G) {
	int local_size = 0;
	int th_id = local_size;
	local_size += state_ptr->imu_ptr->q()->getSize();
	int p_id = local_size;
	local_size += state_ptr->imu_ptr->p()->getSize();
	int v_id = local_size;
	local_size += state_ptr->imu_ptr->v()->getSize();
	int bg_id = local_size;
	local_size += state_ptr->imu_ptr->bg()->getSize();
	int ba_id = local_size;
	local_size += state_ptr->imu_ptr->ba()->getSize();

	int Dw_id = -1;
	int Da_id = -1;
	int Tg_id = -1;
	int th_imu_acc_id = -1;
	int th_imu_gyr_id = -1;

	if (state_ptr->options.do_calib_imu_intrinsics)
	{
		Dw_id = local_size;
		local_size += state_ptr->calib_imu_dw->getSize();
		Da_id = local_size;
		local_size += state_ptr->calib_imu_da->getSize();

		if (state_ptr->options.do_calib_imu_g_sensitivity)
		{
			Tg_id = local_size;
			local_size += state_ptr->calib_imu_tg->getSize();
		}

		if (state_ptr->options.imu_model == ImuModel::KALIBR)
		{
			th_imu_gyr_id = local_size;
			local_size += state_ptr->calib_imu_gyr->getSize();
		}
		else
		{
			th_imu_acc_id = local_size;
			local_size += state_ptr->calib_imu_acc->getSize();
		}
	}

	Eigen::Matrix3d R_k = state_ptr->imu_ptr->getRot();
	Eigen::Vector3d v_k = state_ptr->imu_ptr->getVel();
	Eigen::Vector3d p_k = state_ptr->imu_ptr->getPos();

	if (state_ptr->options.do_fej)
	{
		R_k = state_ptr->imu_ptr->getRotFej();
		v_k = state_ptr->imu_ptr->getVelFej();
		p_k = state_ptr->imu_ptr->getPosFej();
	}

	Eigen::Matrix3d dR_temp = quatType::quatToRot(new_q) * R_k.transpose();

	Eigen::Matrix3d Dw = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_dw->value());
	Eigen::Matrix3d Da = state::Dm(state_ptr->options.imu_model, state_ptr->calib_imu_da->value());
	Eigen::Matrix3d Tg = state::Tg(state_ptr->calib_imu_tg->value());
	Eigen::Matrix3d R_imu_acc = state_ptr->calib_imu_acc->getRot();
	Eigen::Matrix3d R_imu_gyr = state_ptr->calib_imu_gyr->getRot();
	Eigen::Vector3d acc_k = R_imu_acc * Da * acc_uncorrected;
	Eigen::Vector3d gyr_k = R_imu_gyr * Dw * gyr_uncorrected;

	Eigen::Matrix3d Xi_1 = Xi_sum.block<3, 3>(0, 3);
	Eigen::Matrix3d Xi_2 = Xi_sum.block<3, 3>(0, 6);
	Eigen::Matrix3d Jr_temp = Xi_sum.block<3, 3>(0, 9);
	Eigen::Matrix3d Xi_3 = Xi_sum.block<3, 3>(0, 12);
	Eigen::Matrix3d Xi_4 = Xi_sum.block<3, 3>(0, 15);

	F.block<3, 3>(th_id, th_id) = dR_temp;
	F.block<3, 3>(p_id, th_id) = - quatType::skewSymmetric(new_p - p_k - v_k * dt + 0.5 * gravity * dt * dt) * R_k.transpose();
	F.block<3, 3>(v_id, th_id) = - quatType::skewSymmetric(new_v - v_k + gravity * dt) * R_k.transpose();

	F.block<3, 3>(p_id, p_id).setIdentity();

	F.block<3, 3>(p_id, v_id) = Eigen::Matrix3d::Identity() * dt;
	F.block<3, 3>(v_id, v_id).setIdentity();

	F.block<3, 3>(th_id, bg_id) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw;
	F.block<3, 3>(p_id, bg_id) = R_k.transpose() * Xi_4 * R_imu_gyr * Dw;
	F.block<3, 3>(v_id, bg_id) = R_k.transpose() * Xi_3 * R_imu_gyr * Dw;
	F.block<3, 3>(bg_id, bg_id).setIdentity();

	F.block<3, 3>(th_id, ba_id) = dR_temp * Jr_temp * dt * R_imu_gyr * Dw * Tg * R_imu_acc * Da;
	F.block<3, 3>(p_id, ba_id) = - R_k.transpose() * (Xi_2 + Xi_4 * R_imu_gyr * Dw * Tg) * R_imu_acc * Da;
	F.block<3, 3>(v_id, ba_id) = - R_k.transpose() * (Xi_1 + Xi_3 * R_imu_gyr * Dw * Tg) * R_imu_acc * Da;
	F.block<3, 3>(ba_id, ba_id).setIdentity();

	if (Dw_id != -1)
	{
		Eigen::MatrixXd H_Dw = computeDwH(state_ptr, gyr_uncorrected);
		F.block(th_id, Dw_id, 3, state_ptr->calib_imu_dw->getSize()) = dR_temp * Jr_temp * dt * R_imu_gyr * H_Dw;
		F.block(p_id, Dw_id, 3, state_ptr->calib_imu_dw->getSize()) = - R_k.transpose() * Xi_4 * R_imu_gyr * H_Dw;
		F.block(v_id, Dw_id, 3, state_ptr->calib_imu_dw->getSize()) = - R_k.transpose() * Xi_3 * R_imu_gyr * H_Dw;
		F.block(Dw_id, Dw_id, state_ptr->calib_imu_dw->getSize(), state_ptr->calib_imu_dw->getSize()).setIdentity();
	}

	if (Da_id != -1)
	{
		Eigen::MatrixXd H_Da = computeDaH(state_ptr, acc_uncorrected);
		F.block(th_id, Da_id, 3, state_ptr->calib_imu_da->getSize()) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw * Tg * R_imu_acc * H_Da;
		F.block(p_id, Da_id, 3, state_ptr->calib_imu_da->getSize()) = R_k.transpose() * (Xi_2 + Xi_4 * R_imu_gyr * Dw * Tg) * R_imu_acc * H_Da;
		F.block(v_id, Da_id, 3, state_ptr->calib_imu_da->getSize()) = R_k.transpose() * (Xi_1 + Xi_3 * R_imu_gyr * Dw * Tg) * R_imu_acc * H_Da;
		F.block(Da_id, Da_id, state_ptr->calib_imu_da->getSize(), state_ptr->calib_imu_da->getSize()).setIdentity();
	}

	if (Tg_id != -1)
	{
		Eigen::MatrixXd H_Tg = computeTgH(state_ptr, acc_k);
		F.block(th_id, Tg_id, 3, state_ptr->calib_imu_tg->getSize()) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw * H_Tg;
		F.block(p_id, Tg_id, 3, state_ptr->calib_imu_tg->getSize()) = R_k.transpose() * Xi_4 * R_imu_gyr * Dw * H_Tg;
		F.block(v_id, Tg_id, 3, state_ptr->calib_imu_tg->getSize()) = R_k.transpose() * Xi_3 * R_imu_gyr * Dw * H_Tg;
		F.block(Tg_id, Tg_id, state_ptr->calib_imu_tg->getSize(), state_ptr->calib_imu_tg->getSize()).setIdentity();
	}

	if (th_imu_acc_id != -1)
	{
		F.block<3, 3>(th_id, th_imu_acc_id) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw * Tg * quatType::skewSymmetric(acc_k);
		F.block<3, 3>(p_id, th_imu_acc_id) = R_k.transpose() * (Xi_2 + Xi_4 * R_imu_gyr * Dw * Tg) * quatType::skewSymmetric(acc_k);
		F.block<3, 3>(v_id, th_imu_acc_id) = R_k.transpose() * (Xi_1 + Xi_3 * R_imu_gyr * Dw * Tg) * quatType::skewSymmetric(acc_k);
		F.block<3, 3>(th_imu_acc_id, th_imu_acc_id).setIdentity();
	}

	if (th_imu_gyr_id != -1)
	{
		F.block<3, 3>(th_id, th_imu_gyr_id) = dR_temp * Jr_temp * dt * quatType::skewSymmetric(gyr_k);
		F.block<3, 3>(p_id, th_imu_gyr_id) = -R_k.transpose() * Xi_4 * quatType::skewSymmetric(gyr_k);
		F.block<3, 3>(v_id, th_imu_gyr_id) = -R_k.transpose() * Xi_3 * quatType::skewSymmetric(gyr_k);
		F.block<3, 3>(th_imu_gyr_id, th_imu_gyr_id).setIdentity();
	}

	G.block<3, 3>(th_id, 0) = - dR_temp * Jr_temp * dt * R_imu_gyr * Dw;
	G.block<3, 3>(p_id, 0) = R_k.transpose() * Xi_4 * R_imu_gyr * Dw;
	G.block<3, 3>(v_id, 0) = R_k.transpose() * Xi_3 * R_imu_gyr * Dw;
	G.block<3, 3>(th_id, 3) = dR_temp * Jr_temp * dt * R_imu_gyr * Dw * Tg * R_imu_acc * Da;
	G.block<3, 3>(p_id, 3) = -R_k.transpose() * (Xi_2 + Xi_4 * R_imu_gyr * Dw * Tg) * R_imu_acc * Da;
	G.block<3, 3>(v_id, 3) = -R_k.transpose() * (Xi_1 + Xi_3 * R_imu_gyr * Dw * Tg) * R_imu_acc * Da;
	G.block<3, 3>(bg_id, 6) = dt * Eigen::Matrix3d::Identity();
	G.block<3, 3>(ba_id, 9) = dt * Eigen::Matrix3d::Identity();
}
    noiseManager noises;
    std::vector<imuData> v_imu_data;
    Eigen::Vector3d gravity;
    std::atomic<bool> cache_imu_valid;
    double cache_state_time;
    Eigen::MatrixXd cache_state_est;
    Eigen::MatrixXd cache_state_covariance;
    double cache_t_off;
  public:
    updaterMsckf(updaterOptions &options_, featureInitializerOptions &feature_init_options_) {
	options.sigma_pix_sq = std::pow(options.sigma_pix, 2);

	initializer_feature = std::shared_ptr<featureInitializer>(new featureInitializer(feature_init_options_));

	for (int i = 1; i < 500; i++)
	{
		boost::math::chi_squared chi_squared_dist(i);
		chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95);
	}
}
    void update(std::shared_ptr<state> state_ptr, std::vector<std::shared_ptr<feature>> &feature_vec) {
	if (feature_vec.empty())
		return;

	boost::posix_time::ptime rT0, rT1, rT2, rT3;
	rT0 = boost::posix_time::microsec_clock::local_time();

	std::vector<double> clone_times;
	for (const auto &clone_imu : state_ptr->clones_imu)
	{
		clone_times.emplace_back(clone_imu.first);
	}

	auto it0 = feature_vec.begin();
	while (it0 != feature_vec.end())
	{
		(*it0)->cleanOldMeasurements(clone_times);

		int count_meas = 0;
		for (const auto &pair : (*it0)->timestamps)
		{
			count_meas += (*it0)->timestamps[pair.first].size();
		}

		std::shared_ptr<mapPoint> map_point = state_ptr->map_points.at((*it0)->feature_id);
		int required_meas = 2;

		if (count_meas < 1)
		{
			(*it0)->to_delete = true;
			it0 = feature_vec.erase(it0);
		}
		else if (count_meas < required_meas)
		{
			it0 = feature_vec.erase(it0);
		}
		else
		{
			it0++;
		}
	}
	rT1 = boost::posix_time::microsec_clock::local_time();

	size_t max_meas_size = 0;
	for (size_t i = 0; i < feature_vec.size(); i++)
	{
		for (const auto &pair : feature_vec.at(i)->timestamps)
		{
			max_meas_size += 2 * feature_vec.at(i)->timestamps[pair.first].size();
		}
	}

	size_t max_hx_size = state_ptr->maxCovSize();

	Eigen::VectorXd res_big = Eigen::VectorXd::Zero(max_meas_size);
	Eigen::MatrixXd Hx_big = Eigen::MatrixXd::Zero(max_meas_size, max_hx_size);
	Eigen::MatrixXd R_big = Eigen::MatrixXd::Identity(max_meas_size, max_meas_size);
	std::unordered_map<std::shared_ptr<baseType>, size_t> Hx_mapping;
	std::vector<std::shared_ptr<baseType>> Hx_order_big;
	size_t count_jacob = 0;
	size_t count_meas = 0;

	auto it2 = feature_vec.begin();
	while (it2 != feature_vec.end())
	{
		assert(state_ptr->map_points.find((*it2)->feature_id) != state_ptr->map_points.end());
		assert(state_ptr->map_points.at((*it2)->feature_id)->feature_id == (*it2)->feature_id);

		std::shared_ptr<mapPoint> map_point = state_ptr->map_points.at((*it2)->feature_id);

		updaterHelper::updaterHelperFeature feat;
		feat.feature_id = (*it2)->feature_id;
		feat.uvs = (*it2)->uvs;
		feat.uvs_norm = (*it2)->uvs_norm;
		feat.timestamps = (*it2)->timestamps;
		feat.frames = (*it2)->frames;
		feat.color = (*it2)->color;

		feat.position_global = map_point->getPointXYZ(false);
		feat.position_global_fej = map_point->getPointXYZ(true);

		Eigen::MatrixXd H_f;
		Eigen::MatrixXd H_x;
		Eigen::VectorXd res;
		std::vector<std::shared_ptr<baseType>> Hx_order;

		updaterHelper::getFeatureJacobianFull(state_ptr, feat, H_f, H_x, res, Hx_order);

		Eigen::MatrixXd H_xf = H_x;

		H_xf.conservativeResize(H_x.rows(), H_x.cols() + H_f.cols());
		H_xf.block(0, H_x.cols(), H_x.rows(), H_f.cols()) = H_f;

		std::vector<std::shared_ptr<baseType>> Hxf_order = Hx_order;
		Hxf_order.push_back(map_point);

		Eigen::MatrixXd P_marg = stateHelper::getMarginalCovariance(state_ptr, Hxf_order);
		Eigen::MatrixXd S = H_xf * P_marg * H_xf.transpose();
		double sigma_pix_sq = options_slam.sigma_pix_sq;
		S.diagonal() += sigma_pix_sq * Eigen::VectorXd::Ones(S.rows());
		double chi2 = res.dot(S.llt().solve(res));

		double chi2_check;
		if (res.rows() < 500)
		{
			chi2_check = chi_squared_table[res.rows()];
		}
		else
		{
			boost::math::chi_squared chi_squared_dist(res.rows());
			chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
			std::cout << "chi2_check over the residual limit - " << (int)res.rows() << std::endl;
		}

		double chi2_multipler = options_slam.chi2_multipler;

		if (chi2 > chi2_multipler * chi2_check)
		{
			map_point->update_fail_count++;

			(*it2)->to_delete = true;
			it2 = feature_vec.erase(it2);
			continue;
		}

		size_t count_hx = 0;
		for (const auto &var : Hxf_order)
		{
			if (Hx_mapping.find(var) == Hx_mapping.end())
			{
				Hx_mapping.insert({var, count_jacob});
				Hx_order_big.push_back(var);
				count_jacob += var->getSize();
			}

			Hx_big.block(count_meas, Hx_mapping[var], H_xf.rows(), var->getSize()) = H_xf.block(0, count_hx, H_xf.rows(), var->getSize());
			count_hx += var->getSize();
		}

		R_big.block(count_meas, count_meas, res.rows(), res.rows()) *= sigma_pix_sq;

		res_big.block(count_meas, 0, res.rows(), 1) = res;
		count_meas += res.rows();
		it2++;
	}
	rT2 = boost::posix_time::microsec_clock::local_time();

	for (size_t f = 0; f < feature_vec.size(); f++)
	{
		if ((feature_vec[f]->timestamps.find(0) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(0).empty() && feature_vec[f]->timestamps.at(0).back() > clone_times.at(clone_times.size() - 1) + 1e-5) ||
			(feature_vec[f]->timestamps.find(1) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(1).empty() && feature_vec[f]->timestamps.at(1).back() > clone_times.at(clone_times.size() - 1) + 1e-5))
		{
			if ((feature_vec[f]->timestamps.find(0) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(0).empty() && feature_vec[f]->timestamps.at(0).front() < clone_times.at(clone_times.size() - 3) - 1e-5) ||
			(feature_vec[f]->timestamps.find(1) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(1).empty() && feature_vec[f]->timestamps.at(1).front() < clone_times.at(clone_times.size() - 3) - 1e-5))
				feature_vec[f]->to_delete = true;
		}
		else
		{
			feature_vec[f]->to_delete = true;
		}
	}

	if (count_meas < 1)
	{
		return;
	}
	assert(count_meas <= max_meas_size);
	assert(count_jacob <= max_hx_size);
	res_big.conservativeResize(count_meas, 1);
	Hx_big.conservativeResize(count_meas, count_jacob);
	R_big.conservativeResize(count_meas, count_meas);

	stateHelper::ekfUpdate(state_ptr, Hx_order_big, Hx_big, res_big, R_big);
	rT3 = boost::posix_time::microsec_clock::local_time();

	// Time test
	/*
	std::cout << std::fixed << "[updaterSlam]: " << (rT1 - rT0).total_microseconds() * 1e-6 << " seconds to clean." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " seconds creating linear system." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " seconds to update ("
		<< (int)feature_vec.size() << " feats of " << (int)Hx_big.rows() << " size)." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT3 - rT1).total_microseconds() * 1e-6 << " seconds total." << std::endl;
	*/
	// Time test
}
  protected:
    updaterOptions options;
    std::shared_ptr<featureInitializer> initializer_feature;
    std::map<int, double> chi_squared_table;
  public:
    updaterSlam(updaterOptions &options_slam_, featureInitializerOptions &feat_init_options_, std::shared_ptr<featureDatabase> database_) {
	options_slam.sigma_pix_sq = std::pow(options_slam.sigma_pix, 2);

	initializer_feature = std::shared_ptr<featureInitializer>(new featureInitializer(feat_init_options_));

	for (int i = 1; i < 500; i++)
	{
		boost::math::chi_squared chi_squared_dist(i);
		chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95);
	}
}
    void update(std::shared_ptr<state> state_ptr, voxelHashMap &voxel_map, std::vector<std::shared_ptr<feature>> &feature_vec) {
	if (feature_vec.empty())
		return;

	boost::posix_time::ptime rT0, rT1, rT2, rT3;
	rT0 = boost::posix_time::microsec_clock::local_time();

	std::vector<double> clone_times;
	for (const auto &clone_imu : state_ptr->clones_imu)
	{
		clone_times.emplace_back(clone_imu.first);
	}

	auto it0 = feature_vec.begin();
	while (it0 != feature_vec.end())
	{
		(*it0)->cleanOldMeasurements(clone_times);

		int count_meas = 0;
		for (const auto &pair : (*it0)->timestamps)
		{
			count_meas += (*it0)->timestamps[pair.first].size();
		}

		std::shared_ptr<mapPoint> map_point = state_ptr->map_points.at((*it0)->feature_id);
		int required_meas = 2;

		if (count_meas < 1)
		{
			(*it0)->to_delete = true;
			it0 = feature_vec.erase(it0);
		}
		else if (count_meas < required_meas)
		{
			it0 = feature_vec.erase(it0);
		}
		else
		{
			it0++;
		}
	}
	rT1 = boost::posix_time::microsec_clock::local_time();

	size_t max_meas_size = 0;
	for (size_t i = 0; i < feature_vec.size(); i++)
	{
		for (const auto &pair : feature_vec.at(i)->timestamps)
		{
			max_meas_size += 2 * feature_vec.at(i)->timestamps[pair.first].size();
		}
	}

	size_t max_hx_size = state_ptr->maxCovSize();

	Eigen::VectorXd res_big = Eigen::VectorXd::Zero(max_meas_size);
	Eigen::MatrixXd Hx_big = Eigen::MatrixXd::Zero(max_meas_size, max_hx_size);
	Eigen::MatrixXd R_big = Eigen::MatrixXd::Identity(max_meas_size, max_meas_size);
	std::unordered_map<std::shared_ptr<baseType>, size_t> Hx_mapping;
	std::vector<std::shared_ptr<baseType>> Hx_order_big;
	size_t count_jacob = 0;
	size_t count_meas = 0;

	auto it2 = feature_vec.begin();
	while (it2 != feature_vec.end())
	{
		assert(state_ptr->map_points.find((*it2)->feature_id) != state_ptr->map_points.end());
		assert(state_ptr->map_points.at((*it2)->feature_id)->feature_id == (*it2)->feature_id);

		std::shared_ptr<mapPoint> map_point = state_ptr->map_points.at((*it2)->feature_id);

		updaterHelper::updaterHelperFeature feat;
		feat.feature_id = (*it2)->feature_id;
		feat.uvs = (*it2)->uvs;
		feat.uvs_norm = (*it2)->uvs_norm;
		feat.timestamps = (*it2)->timestamps;
		feat.frames = (*it2)->frames;
		feat.color = (*it2)->color;

		feat.position_global = map_point->getPointXYZ(false);
		feat.position_global_fej = map_point->getPointXYZ(true);

		Eigen::MatrixXd H_f;
		Eigen::MatrixXd H_x;
		Eigen::VectorXd res;
		std::vector<std::shared_ptr<baseType>> Hx_order;

		updaterHelper::getFeatureJacobianFull(state_ptr, feat, H_f, H_x, res, Hx_order);

		Eigen::MatrixXd H_xf = H_x;

		H_xf.conservativeResize(H_x.rows(), H_x.cols() + H_f.cols());
		H_xf.block(0, H_x.cols(), H_x.rows(), H_f.cols()) = H_f;

		std::vector<std::shared_ptr<baseType>> Hxf_order = Hx_order;
		Hxf_order.push_back(map_point);

		Eigen::MatrixXd P_marg = stateHelper::getMarginalCovariance(state_ptr, Hxf_order);
		Eigen::MatrixXd S = H_xf * P_marg * H_xf.transpose();
		double sigma_pix_sq = options_slam.sigma_pix_sq;
		S.diagonal() += sigma_pix_sq * Eigen::VectorXd::Ones(S.rows());
		double chi2 = res.dot(S.llt().solve(res));

		double chi2_check;
		if (res.rows() < 500)
		{
			chi2_check = chi_squared_table[res.rows()];
		}
		else
		{
			boost::math::chi_squared chi_squared_dist(res.rows());
			chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
			std::cout << "chi2_check over the residual limit - " << (int)res.rows() << std::endl;
		}

		double chi2_multipler = options_slam.chi2_multipler;

		if (chi2 > chi2_multipler * chi2_check)
		{
			map_point->update_fail_count++;

			(*it2)->to_delete = true;
			it2 = feature_vec.erase(it2);
			continue;
		}

		size_t count_hx = 0;
		for (const auto &var : Hxf_order)
		{
			if (Hx_mapping.find(var) == Hx_mapping.end())
			{
				Hx_mapping.insert({var, count_jacob});
				Hx_order_big.push_back(var);
				count_jacob += var->getSize();
			}

			Hx_big.block(count_meas, Hx_mapping[var], H_xf.rows(), var->getSize()) = H_xf.block(0, count_hx, H_xf.rows(), var->getSize());
			count_hx += var->getSize();
		}

		R_big.block(count_meas, count_meas, res.rows(), res.rows()) *= sigma_pix_sq;

		res_big.block(count_meas, 0, res.rows(), 1) = res;
		count_meas += res.rows();
		it2++;
	}
	rT2 = boost::posix_time::microsec_clock::local_time();

	for (size_t f = 0; f < feature_vec.size(); f++)
	{
		if ((feature_vec[f]->timestamps.find(0) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(0).empty() && feature_vec[f]->timestamps.at(0).back() > clone_times.at(clone_times.size() - 1) + 1e-5) ||
			(feature_vec[f]->timestamps.find(1) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(1).empty() && feature_vec[f]->timestamps.at(1).back() > clone_times.at(clone_times.size() - 1) + 1e-5))
		{
			if ((feature_vec[f]->timestamps.find(0) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(0).empty() && feature_vec[f]->timestamps.at(0).front() < clone_times.at(clone_times.size() - 3) - 1e-5) ||
			(feature_vec[f]->timestamps.find(1) != feature_vec[f]->timestamps.end() && !feature_vec[f]->timestamps.at(1).empty() && feature_vec[f]->timestamps.at(1).front() < clone_times.at(clone_times.size() - 3) - 1e-5))
				feature_vec[f]->to_delete = true;
		}
		else
		{
			feature_vec[f]->to_delete = true;
		}
	}

	if (count_meas < 1)
	{
		return;
	}
	assert(count_meas <= max_meas_size);
	assert(count_jacob <= max_hx_size);
	res_big.conservativeResize(count_meas, 1);
	Hx_big.conservativeResize(count_meas, count_jacob);
	R_big.conservativeResize(count_meas, count_meas);

	stateHelper::ekfUpdate(state_ptr, Hx_order_big, Hx_big, res_big, R_big);
	rT3 = boost::posix_time::microsec_clock::local_time();

	// Time test
	/*
	std::cout << std::fixed << "[updaterSlam]: " << (rT1 - rT0).total_microseconds() * 1e-6 << " seconds to clean." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " seconds creating linear system." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " seconds to update ("
		<< (int)feature_vec.size() << " feats of " << (int)Hx_big.rows() << " size)." << std::endl;
	std::cout << std::fixed << "[updaterSlam]: " << (rT3 - rT1).total_microseconds() * 1e-6 << " seconds total." << std::endl;
	*/
	// Time test
}
    void delayedInit(std::shared_ptr<state> state_ptr, voxelHashMap &voxel_map, std::vector<std::shared_ptr<feature>> &feature_vec, pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_history) {
	if (feature_vec.empty())
		return;

	boost::posix_time::ptime rT0, rT1, rT2, rT3;
	rT0 = boost::posix_time::microsec_clock::local_time();

	std::vector<double> clone_times;
	for (const auto &clone_imu : state_ptr->clones_imu)
	{
		clone_times.emplace_back(clone_imu.first);
	}

	auto it0 = feature_vec.begin();
	while (it0 != feature_vec.end())
	{
		(*it0)->cleanOldMeasurements(clone_times);

		int count_meas = 0;
		for (const auto &pair : (*it0)->timestamps)
		{
			count_meas += (*it0)->timestamps[pair.first].size();
		}

		if (count_meas < 2)
		{
			(*it0)->to_delete = true;
			it0 = feature_vec.erase(it0);
		}
		else
		{
			it0++;
		}
	}
	rT1 = boost::posix_time::microsec_clock::local_time();

	std::unordered_map<size_t, std::unordered_map<double, featureInitializer::clonePose>> clones_cam;
	for (const auto &clone_calib : state_ptr->calib_cam_imu)
	{
		std::unordered_map<double, featureInitializer::clonePose> clones_cami;
		for (const auto &clone_imu : state_ptr->clones_imu)
		{
			Eigen::Matrix<double, 3, 3> R_GtoCi = clone_calib.second->getRot() * clone_imu.second->getRot();
			Eigen::Matrix<double, 3, 1> p_CioinG = clone_imu.second->getPos() - R_GtoCi.transpose() * clone_calib.second->getPos();

			clones_cami.insert({clone_imu.first, featureInitializer::clonePose(R_GtoCi, p_CioinG)});
		}

		clones_cam.insert({clone_calib.first, clones_cami});
	}

	auto it1 = feature_vec.begin();
	while (it1 != feature_vec.end())
	{
		bool success_tri = true;
		success_tri = initializer_feature->singleTriangulation(*it1, clones_cam);

		bool success_refine = true;
		if (initializer_feature->config().refine_features)
		{
			success_refine = initializer_feature->singleGaussnewton(*it1, clones_cam);
		}

		if (!success_tri || !success_refine)
		{
			(*it1)->to_delete = true;
			it1 = feature_vec.erase(it1);
			continue;
		}
		it1++;
	}
	rT2 = boost::posix_time::microsec_clock::local_time();

	auto it2 = feature_vec.begin();
	while (it2 != feature_vec.end())
	{
		updaterHelper::updaterHelperFeature feat;
		feat.feature_id = (*it2)->feature_id;
		feat.uvs = (*it2)->uvs;
		feat.uvs_norm = (*it2)->uvs_norm;
		feat.timestamps = (*it2)->timestamps;

		feat.frames = (*it2)->frames;
		feat.color = (*it2)->color;
		assert(feat.color[0].size() == feat.uvs[0].size());
		assert(feat.color[1].size() == feat.uvs[1].size());
		assert(feat.color[(*it2)->anchor_cam_id].back() >= 0);

		feat.anchor_cam_id = (*it2)->anchor_cam_id;
		feat.anchor_clone_timestamp = (*it2)->anchor_clone_timestamp;

		feat.position_global = (*it2)->position_global;
		feat.position_global_fej = (*it2)->position_global;

		Eigen::MatrixXd H_f;
		Eigen::MatrixXd H_x;
		Eigen::VectorXd res;
		std::vector<std::shared_ptr<baseType>> Hx_order;

		updaterHelper::getFeatureJacobianFull(state_ptr, feat, H_f, H_x, res, Hx_order);

		auto map_point_ptr = std::make_shared<mapPoint>();
		map_point_ptr->feature_id = feat.feature_id;
		map_point_ptr->unique_camera_id = (*it2)->anchor_cam_id;

		map_point_ptr->color = feat.color[feat.anchor_cam_id].back();
		map_point_ptr->host_frame = feat.frames[(*it2)->anchor_cam_id].back();
		map_point_ptr->anchor_cam_id = feat.anchor_cam_id;
		map_point_ptr->anchor_clone_timestamp = feat.anchor_clone_timestamp;

		map_point_ptr->setPointXYZ(feat.position_global, false);
		map_point_ptr->setPointXYZ(feat.position_global_fej, true);

		double sigma_pix_sq = options_slam.sigma_pix_sq;
		Eigen::MatrixXd R = sigma_pix_sq * Eigen::MatrixXd::Identity(res.rows(), res.rows());

		double chi2_multipler = options_slam.chi2_multipler;

		if (stateHelper::initialize(state_ptr, map_point_ptr, Hx_order, H_x, H_f, R, res, chi2_multipler))
		{
			bool add = mapManagement::addPointToVoxel(voxel_map, map_point_ptr, state_ptr->options.voxel_size, state_ptr->options.max_num_points_in_voxel,
				state_ptr->options.min_distance_points, voxels_history);

			if (add)
			{
				state_ptr->map_points.insert({(*it2)->feature_id, map_point_ptr});
				(*it2)->to_delete = true;
				it2++;
			}
			else
			{
				(*it2)->to_delete = true;
				it2 = feature_vec.erase(it2);
			}
		}
		else
		{
			(*it2)->to_delete = true;
			it2 = feature_vec.erase(it2);
		}
	}
	rT3 = boost::posix_time::microsec_clock::local_time();

	// Time test
	/*
	if (!feature_vec.empty())
	{
		std::cout << std::fixed << "[updaterSlam::delayedInit]: " << (rT1 - rT0).total_microseconds() * 1e-6 << " seconds to clean." << std::endl;
		std::cout << std::fixed << "[updaterSlam::delayedInit]: " << (rT2 - rT1).total_microseconds() * 1e-6 << " seconds to triangulate." << std::endl;
		std::cout << std::fixed << "[updaterSlam::delayedInit]: " << (rT3 - rT2).total_microseconds() * 1e-6 << " seconds initialize (" << (int)feature_vec.size() << " features)." << std::endl;
		std::cout << std::fixed << "[updaterSlam::delayedInit]: " << (rT3 - rT1).total_microseconds() * 1e-6 << " seconds total." << std::endl;
	}
	*/
	// Time test
}
  protected:
    updaterOptions options_slam;
    std::shared_ptr<featureInitializer> initializer_feature;
    std::shared_ptr<featureDatabase> database;
    std::map<int, double> chi_squared_table;
};

class baseType {
  private:
    stateHelper() {

}
  public:
    static void ekfPropagation(std::shared_ptr<state> state_ptr, const std::vector<std::shared_ptr<baseType>> &order_new,
		const std::vector<std::shared_ptr<baseType>> &order_old, const Eigen::MatrixXd &phi, const Eigen::MatrixXd &Q) {
	if (order_new.empty() || order_old.empty())
	{
		std::cout << "[stateHelper::ekfPropagation]: Called with empty variable arrays!" << std::endl;
		std::exit(EXIT_FAILURE);
	}

	int size_order_new = order_new.at(0)->getSize();
	for (size_t i = 0; i < order_new.size() - 1; i++)
	{
		if (order_new.at(i)->getId() + order_new.at(i)->getSize() != order_new.at(i + 1)->getId())
		{
			std::cout << "[stateHelper::ekfPropagation]: Called with non-contiguous state elements!" << std::endl;
			std::cout << "[stateHelper::ekfPropagation]: This code only support a state transition which is in the same order as the state." << std::endl;
			std::exit(EXIT_FAILURE);
		}
		size_order_new += order_new.at(i + 1)->getSize();
	}

	int size_order_old = order_old.at(0)->getSize();
	for (size_t i = 0; i < order_old.size() - 1; i++)
	{
		size_order_old += order_old.at(i + 1)->getSize();
	}

	assert(size_order_new == phi.rows());
	assert(size_order_old == phi.cols());
	assert(size_order_new == Q.cols());
	assert(size_order_new == Q.rows());

	int current_it = 0;
	std::vector<int> phi_id;
	for (const auto &var : order_old)
	{
		phi_id.push_back(current_it);
		current_it += var->getSize();
	}

	Eigen::MatrixXd cov_phiT = Eigen::MatrixXd::Zero(state_ptr->cov.rows(), phi.rows());
	for (size_t i = 0; i < order_old.size(); i++)
	{
		std::shared_ptr<baseType> var = order_old.at(i);
		cov_phiT.noalias() += state_ptr->cov.block(0, var->getId(), state_ptr->cov.rows(), var->getSize()) * phi.block(0, phi_id[i], phi.rows(), var->getSize()).transpose();
	}

	Eigen::MatrixXd phi_cov_phiT = Q.selfadjointView<Eigen::Upper>();
	for (size_t i = 0; i < order_old.size(); i++)
	{
		std::shared_ptr<baseType> var = order_old.at(i);
		phi_cov_phiT.noalias() += phi.block(0, phi_id[i], phi.rows(), var->getSize()) * cov_phiT.block(var->getId(), 0, var->getSize(), phi.rows());
	}

	int start_id = order_new.at(0)->getId();
	int phi_size = phi.rows();
	int total_size = state_ptr->cov.rows();
	state_ptr->cov.block(start_id, 0, phi_size, total_size) = cov_phiT.transpose();
	state_ptr->cov.block(0, start_id, total_size, phi_size) = cov_phiT;
	state_ptr->cov.block(start_id, start_id, phi_size, phi_size) = phi_cov_phiT;

	Eigen::VectorXd diags = state_ptr->cov.diagonal();
	bool found_neg = false;
	for (int i = 0; i < diags.rows(); i++)
	{
		if (diags(i) < 0.0)
		{
			std::cout << "[stateHelper::ekfPropagation]: Diagonal at " << i << " is " << diags(i) << std::endl;
			found_neg = true;
		}
	}

	if (found_neg)
	{
		std::exit(EXIT_FAILURE);
	}
}
    static void ekfUpdate(std::shared_ptr<state> state_ptr, const std::vector<std::shared_ptr<baseType>> &H_order,
		const Eigen::MatrixXd &H, const Eigen::VectorXd &res, const Eigen::MatrixXd &R) {
	assert(res.rows() == R.rows());
	assert(H.rows() == res.rows());
	Eigen::MatrixXd M_a = Eigen::MatrixXd::Zero(state_ptr->cov.rows(), res.rows());

	int current_it = 0;
	std::vector<int> H_id;
	for (const auto &meas_var : H_order)
	{
		H_id.push_back(current_it);
		current_it += meas_var->getSize();
	}

	for (const auto &var : state_ptr->variables)
	{
		// Sum up effect of each subjacobian = K_i= \sum_m (P_im Hm^T)
		Eigen::MatrixXd M_i = Eigen::MatrixXd::Zero(var->getSize(), res.rows());
		for (size_t i = 0; i < H_order.size(); i++)
		{
			std::shared_ptr<baseType> meas_var = H_order[i];
			M_i.noalias() += state_ptr->cov.block(var->getId(), meas_var->getId(), var->getSize(), meas_var->getSize()) *
				H.block(0, H_id[i], H.rows(), meas_var->getSize()).transpose();
		}
		M_a.block(var->getId(), 0, var->getSize(), res.rows()) = M_i;
	}

	Eigen::MatrixXd P_small = stateHelper::getMarginalCovariance(state_ptr, H_order);
	Eigen::MatrixXd S = H * P_small * H.transpose() + R;
	Eigen::MatrixXd S_inv = Eigen::MatrixXd::Identity(R.rows(), R.rows());
	S.selfadjointView<Eigen::Upper>().llt().solveInPlace(S_inv);
	Eigen::MatrixXd K = M_a * S_inv.selfadjointView<Eigen::Upper>();

	state_ptr->cov.triangularView<Eigen::Upper>() -= K * M_a.transpose();
	state_ptr->cov = state_ptr->cov.selfadjointView<Eigen::Upper>();

	Eigen::VectorXd diags = state_ptr->cov.diagonal();
	bool found_neg = false;

	for (int i = 0; i < diags.rows(); i++)
	{
		if (diags(i) < 0.0)
		{
			std::cout << "[stateHelper::ekfUpdate: Diagonal at " << i << " is " << diags(i) << std::endl;
			found_neg = true;
		}
	}

	if (found_neg)
	{
		std::exit(EXIT_FAILURE);
	}

	Eigen::VectorXd dx = K * res;
	for (size_t i = 0; i < state_ptr->variables.size(); i++)
	{
		state_ptr->variables.at(i)->update(dx.block(state_ptr->variables.at(i)->getId(), 0, state_ptr->variables.at(i)->getSize(), 1));
	}

	if (state_ptr->options.do_calib_camera_intrinsics)
	{
		for (auto const &calib : state_ptr->cam_intrinsics)
		{
			state_ptr->cam_intrinsics_cameras.at(calib.first)->setValue(calib.second->value());
		}
	}
}
    static void setInitialCovariance(std::shared_ptr<state> state_ptr, const Eigen::MatrixXd &covariance,
		const std::vector<std::shared_ptr<baseType>> &order) {
	int i_index = 0;

	for (size_t i = 0; i < order.size(); i++)
	{
		int k_index = 0;
		for (size_t k = 0; k < order.size(); k++)
		{
			state_ptr->cov.block(order[i]->getId(), order[k]->getId(), order[i]->getSize(), order[k]->getSize()) =
				covariance.block(i_index, k_index, order[i]->getSize(), order[k]->getSize());
			k_index += order[k]->getSize();
		}
		i_index += order[i]->getSize();
	}
	state_ptr->cov = state_ptr->cov.selfadjointView<Eigen::Upper>();
}
    static Eigen::MatrixXd getMarginalCovariance(std::shared_ptr<state> state_ptr, const std::vector<std::shared_ptr<baseType>> &small_variables) {
	int cov_size = 0;
	for (size_t i = 0; i < small_variables.size(); i++)
	{
		cov_size += small_variables[i]->getSize();
	}

	Eigen::MatrixXd small_cov = Eigen::MatrixXd::Zero(cov_size, cov_size);
	int i_index = 0;

	for (size_t i = 0; i < small_variables.size(); i++)
	{
		int k_index = 0;

		for (size_t k = 0; k < small_variables.size(); k++)
		{
			small_cov.block(i_index, k_index, small_variables[i]->getSize(), small_variables[k]->getSize()) =
				state_ptr->cov.block(small_variables[i]->getId(), small_variables[k]->getId(), small_variables[i]->getSize(), small_variables[k]->getSize());
			k_index += small_variables[k]->getSize();
		}

		i_index += small_variables[i]->getSize();
	}

	return small_cov;
}
    static Eigen::MatrixXd getFullCovariance(std::shared_ptr<state> state_ptr) {
	int cov_size = (int)state_ptr->cov.rows();

	Eigen::MatrixXd full_cov = Eigen::MatrixXd::Zero(cov_size, cov_size);

	full_cov.block(0, 0, state_ptr->cov.rows(), state_ptr->cov.rows()) = state_ptr->cov;

	return full_cov;
}
    static void marginalize(std::shared_ptr<state> state_ptr, std::shared_ptr<baseType> marg) {
	if (std::find(state_ptr->variables.begin(), state_ptr->variables.end(), marg) == state_ptr->variables.end())
	{
		std::cout << "[stateHelper::marginalize]: Called on variable that is not in the state." << std::endl;
		std::cout << "[stateHelper::marginalize]: Marginalization, does NOT work on sub-variables yet..." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	int marg_size = marg->getSize();
	int marg_id = marg->getId();
	int x2_size = (int)state_ptr->cov.rows() - marg_id - marg_size;

	Eigen::MatrixXd cov_new(state_ptr->cov.rows() - marg_size, state_ptr->cov.rows() - marg_size);

	cov_new.block(0, 0, marg_id, marg_id) = state_ptr->cov.block(0, 0, marg_id, marg_id);
	cov_new.block(0, marg_id, marg_id, x2_size) = state_ptr->cov.block(0, marg_id + marg_size, marg_id, x2_size);
	cov_new.block(marg_id, 0, x2_size, marg_id) = cov_new.block(0, marg_id, marg_id, x2_size).transpose();
	cov_new.block(marg_id, marg_id, x2_size, x2_size) = state_ptr->cov.block(marg_id + marg_size, marg_id + marg_size, x2_size, x2_size);

	state_ptr->cov = cov_new;
	assert(state_ptr->cov.rows() == cov_new.rows());

	std::vector<std::shared_ptr<baseType>> remaining_variables;

	for (size_t i = 0; i < state_ptr->variables.size(); i++)
	{
		if (state_ptr->variables.at(i) != marg)
		{
			if (state_ptr->variables.at(i)->getId() > marg_id)
			{
				state_ptr->variables.at(i)->setLocalId(state_ptr->variables.at(i)->getId() - marg_size);
			}
			remaining_variables.push_back(state_ptr->variables.at(i));
		}
	}

	marg->setLocalId(-1);
	state_ptr->variables = remaining_variables;
}
    static std::shared_ptr<baseType> clone(std::shared_ptr<state> state_ptr, std::shared_ptr<baseType> variable_to_clone) {
	int total_size = variable_to_clone->getSize();
	int old_size = (int)state_ptr->cov.rows();
	int new_loc = (int)state_ptr->cov.rows();

	state_ptr->cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_size + total_size, old_size + total_size));

	const std::vector<std::shared_ptr<baseType>> new_variables = state_ptr->variables;
	std::shared_ptr<baseType> new_clone = nullptr;

	for (size_t k = 0; k < state_ptr->variables.size(); k++)
	{
		std::shared_ptr<baseType> type_check = state_ptr->variables.at(k)->checkIfSubvariable(variable_to_clone);
		if (state_ptr->variables.at(k) == variable_to_clone)
		{
			type_check = state_ptr->variables.at(k);
		}
		else if (type_check != variable_to_clone)
		{
			continue;
		}

		int old_loc = type_check->getId();

		state_ptr->cov.block(new_loc, new_loc, total_size, total_size) = state_ptr->cov.block(old_loc, old_loc, total_size, total_size);
		state_ptr->cov.block(0, new_loc, old_size, total_size) = state_ptr->cov.block(0, old_loc, old_size, total_size);
		state_ptr->cov.block(new_loc, 0, total_size, old_size) = state_ptr->cov.block(old_loc, 0, total_size, old_size);

		new_clone = type_check->clone();
		new_clone->setLocalId(new_loc);
		break;
	}

	if (new_clone == nullptr)
	{
		std::cout << "[stateHelper::clone]: Called on variable is not in the state." << std::endl;
		std::cout << "[stateHelper::clone]: Ensure that the variable specified is a variable, or sub-variable." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	state_ptr->variables.push_back(new_clone);
	return new_clone;
}
    static bool initialize(std::shared_ptr<state> state_ptr, std::shared_ptr<baseType> new_variable,
		const std::vector<std::shared_ptr<baseType>> &H_order, Eigen::MatrixXd &H_R, Eigen::MatrixXd &H_L,
		Eigen::MatrixXd &R, Eigen::VectorXd &res, double chi_2_mult) {
	if (std::find(state_ptr->variables.begin(), state_ptr->variables.end(), new_variable) != state_ptr->variables.end())
	{
		std::cout << "[stateHelper::initializeInvertible]: Called on variable that is already in the state." << std::endl;
		std::cout << "[stateHelper::initializeInvertible]: Found this variable at " << new_variable->getId() << " in covariance." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	assert(R.rows() == R.cols());
	assert(R.rows() > 0);
	for (int r = 0; r < R.rows(); r++)
	{
		for (int c = 0; c < R.cols(); c++)
		{
			if (r == c && R(0, 0) != R(r, c))
			{
				std::cout << "[stateHelper::initialize]: Your noise is not isotropic!" << std::endl;
				std::cout << "[stateHelper::initialize]: Found a value of " << R(r, c) << " verses value of " << R(0, 0) << "." << std::endl;
				std::exit(EXIT_FAILURE);
			}
			else if (r != c && R(r, c) != 0.0)
			{
				std::cout << "[stateHelper::initialize]: Your noise is not diagonal!" << std::endl;
				std::cout << "[stateHelper::initialize]: Found a value of " << R(r, c) << " at row " << r << " and column " << c <<"." << std::endl;
				std::exit(EXIT_FAILURE);
			}
		}
	}

	size_t new_var_size = new_variable->getSize();
	assert((int)new_var_size == H_L.cols());

	Eigen::JacobiRotation<double> tempHo_GR;
	for (int n = 0; n < H_L.cols(); ++n)
	{
		for (int m = (int)H_L.rows() - 1; m > n; m--)
		{
			tempHo_GR.makeGivens(H_L(m - 1, n), H_L(m, n));
			(H_L.block(m - 1, n, 2, H_L.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
			(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
			(H_R.block(m - 1, 0, 2, H_R.cols())).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
		}
	}

	Eigen::MatrixXd Hxinit = H_R.block(0, 0, new_var_size, H_R.cols());
	Eigen::MatrixXd H_finit = H_L.block(0, 0, new_var_size, new_var_size);
	Eigen::VectorXd resinit = res.block(0, 0, new_var_size, 1);
	Eigen::MatrixXd Rinit = R.block(0, 0, new_var_size, new_var_size);

	Eigen::MatrixXd Hup = H_R.block(new_var_size, 0, H_R.rows() - new_var_size, H_R.cols());
	Eigen::VectorXd resup = res.block(new_var_size, 0, res.rows() - new_var_size, 1);
	Eigen::MatrixXd Rup = R.block(new_var_size, new_var_size, R.rows() - new_var_size, R.rows() - new_var_size);

	Eigen::MatrixXd P_up = stateHelper::getMarginalCovariance(state_ptr, H_order);
	assert(Rup.rows() == Hup.rows());
	assert(Hup.cols() == P_up.cols());
	Eigen::MatrixXd S = Hup * P_up * Hup.transpose() + Rup;
	double chi2 = resup.dot(S.llt().solve(resup));

	boost::math::chi_squared chi_squared_dist(res.rows());
	double chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
	if (chi2 > chi_2_mult * chi2_check)
	{
		return false;
	}

	stateHelper::initializeInvertible(state_ptr, new_variable, H_order, Hxinit, H_finit, Rinit, resinit);

	if (Hup.rows() > 0)
	{
		stateHelper::ekfUpdate(state_ptr, H_order, Hup, resup, Rup);
	}
	return true;
}
    static void initializeInvertible(std::shared_ptr<state> state_ptr, std::shared_ptr<baseType> new_variable,
		const std::vector<std::shared_ptr<baseType>> &H_order, const Eigen::MatrixXd &H_R,
		const Eigen::MatrixXd &H_L, const Eigen::MatrixXd &R, const Eigen::VectorXd &res) {
	if (std::find(state_ptr->variables.begin(), state_ptr->variables.end(), new_variable) != state_ptr->variables.end())
	{
		std::cout << "[stateHelper::initializeInvertible]: Called on variable that is already in the state." << std::endl;
		std::cout << "[stateHelper::initializeInvertible]: Found this variable at " << new_variable->getId() << " in covariance." << std::endl;
		std::exit(EXIT_FAILURE);
	}

	assert(R.rows() == R.cols());
	assert(R.rows() > 0);
	for (int r = 0; r < R.rows(); r++)
	{
		for (int c = 0; c < R.cols(); c++)
		{
			if (r == c && R(0, 0) != R(r, c))
			{
				std::cout << "[stateHelper::initializeInvertible]: Your noise is not isotropic!" << std::endl;
				std::cout << "[stateHelper::initializeInvertible]: Found a value of " << R(r, c) << " verses value of " << R(0, 0) << "." << std::endl;
				std::exit(EXIT_FAILURE);
			}
			else if (r != c && R(r, c) != 0.0)
			{
				std::cout << "[stateHelper::initializeInvertible]: Your noise is not diagonal!" << std::endl;
				std::cout << "[stateHelper::initializeInvertible]: Found a value of " << R(r, c) << " at row " << r << " and column " << c << "." << std::endl;
				std::exit(EXIT_FAILURE);
			}
		}
	}

	assert(res.rows() == R.rows());
	assert(H_L.rows() == res.rows());
	assert(H_L.rows() == H_R.rows());
	Eigen::MatrixXd M_a = Eigen::MatrixXd::Zero(state_ptr->cov.rows(), res.rows());

	int current_it = 0;
	std::vector<int> H_id;
	for (const auto &meas_var : H_order) {
		H_id.push_back(current_it);
		current_it += meas_var->getSize();
	}

	for (const auto &var : state_ptr->variables)
	{
		Eigen::MatrixXd M_i = Eigen::MatrixXd::Zero(var->getSize(), res.rows());
		for (size_t i = 0; i < H_order.size(); i++)
		{
			std::shared_ptr<baseType> meas_var = H_order.at(i);
			M_i += state_ptr->cov.block(var->getId(), meas_var->getId(), var->getSize(), meas_var->getSize()) *
				H_R.block(0, H_id[i], H_R.rows(), meas_var->getSize()).transpose();
		}
		M_a.block(var->getId(), 0, var->getSize(), res.rows()) = M_i;
	}

	Eigen::MatrixXd P_small = stateHelper::getMarginalCovariance(state_ptr, H_order);

	Eigen::MatrixXd M(H_R.rows(), H_R.rows());
	M.triangularView<Eigen::Upper>() = H_R * P_small * H_R.transpose();
	M.triangularView<Eigen::Upper>() += R;

	assert(H_L.rows() == H_L.cols());
	assert(H_L.rows() == new_variable->getSize());
	Eigen::MatrixXd H_Linv = H_L.inverse();
	Eigen::MatrixXd P_LL = H_Linv * M.selfadjointView<Eigen::Upper>() * H_Linv.transpose();

	size_t oldSize = state_ptr->cov.rows();
	state_ptr->cov.conservativeResizeLike(Eigen::MatrixXd::Zero(oldSize + new_variable->getSize(), oldSize + new_variable->getSize()));
	state_ptr->cov.block(0, oldSize, oldSize, new_variable->getSize()).noalias() = -M_a * H_Linv.transpose();
	state_ptr->cov.block(oldSize, 0, new_variable->getSize(), oldSize) = state_ptr->cov.block(0, oldSize, oldSize, new_variable->getSize()).transpose();
	state_ptr->cov.block(oldSize, oldSize, new_variable->getSize(), new_variable->getSize()) = P_LL;

	new_variable->update(H_Linv * res);
	new_variable->setLocalId(oldSize);
	state_ptr->variables.push_back(new_variable);
}
    static void augmentClone(std::shared_ptr<state> state_ptr, Eigen::Matrix<double, 3, 1> last_gyr) {
	if (state_ptr->clones_imu.find(state_ptr->timestamp) != state_ptr->clones_imu.end())
	{
		std::exit(EXIT_FAILURE);
	}

	std::shared_ptr<baseType> pose_temp = stateHelper::clone(state_ptr, state_ptr->imu_ptr->pose());

	std::shared_ptr<poseJpl> pose = std::dynamic_pointer_cast<poseJpl>(pose_temp);
	if (pose == nullptr)
	{
		std::exit(EXIT_FAILURE);
	}

	state_ptr->clones_imu[state_ptr->timestamp] = pose;

	if (state_ptr->options.do_calib_camera_timeoffset)
	{
		Eigen::Matrix<double, 6, 1> dnc_dt = Eigen::MatrixXd::Zero(6, 1);
		dnc_dt.block(0, 0, 3, 1) = last_gyr;
		dnc_dt.block(3, 0, 3, 1) = state_ptr->imu_ptr->getVel();

		state_ptr->cov.block(0, pose->getId(), state_ptr->cov.rows(), 6) +=
			state_ptr->cov.block(0, state_ptr->calib_dt_imu_cam->getId(), state_ptr->cov.rows(), 1) * dnc_dt.transpose();
		state_ptr->cov.block(pose->getId(), 0, 6, state_ptr->cov.rows()) +=
			dnc_dt * state_ptr->cov.block(state_ptr->calib_dt_imu_cam->getId(), 0, 1, state_ptr->cov.rows());
	}
}
    static void marginalizeOldClone(std::shared_ptr<state> state_ptr) {
	if ((int)state_ptr->clones_imu.size() > state_ptr->options.max_clone_size)
	{
		double marginal_time = state_ptr->margtimestep();

		assert(marginal_time != INFINITY);
		stateHelper::marginalize(state_ptr, state_ptr->clones_imu.at(marginal_time));

		state_ptr->clones_imu.erase(marginal_time);

		assert(state_ptr->clones_frame.at(marginal_time)->v_feat_ptr[0].size() == 0);
		assert(state_ptr->clones_frame.at(marginal_time)->v_feat_ptr[1].size() == 0);
		state_ptr->clones_frame.at(marginal_time)->release();
		state_ptr->clones_frame.erase(marginal_time);
	}
}
    static void marginalizeNewClone(std::shared_ptr<state> state_ptr) {
	if ((int)state_ptr->clones_imu.size() > state_ptr->options.max_clone_size)
	{
		std::vector<double> clone_times;
		for (const auto &clone_imu : state_ptr->clones_imu)
		{
			clone_times.emplace_back(clone_imu.first);
		}

		double marginal_time = clone_times.at(clone_times.size() - 2);

		assert(marginal_time != INFINITY);
		stateHelper::marginalize(state_ptr, state_ptr->clones_imu.at(marginal_time));

		std::shared_ptr<frame> fh_temp = state_ptr->clones_frame.at(marginal_time);

		if (fh_temp->v_feat_ptr.find(0) != fh_temp->v_feat_ptr.end())
		{
			auto it0 = fh_temp->v_feat_ptr.at(0).begin();

			while (it0 != fh_temp->v_feat_ptr.at(0).end())
			{
				std::vector<double> marginal_timestamps = {marginal_time};
				(*it0).second->cleanInvalidMeasurements(marginal_timestamps);
				it0 = fh_temp->v_feat_ptr.at(0).begin();
			}
		}

		if (fh_temp->v_feat_ptr.find(1) != fh_temp->v_feat_ptr.end())
		{
			auto it1 = fh_temp->v_feat_ptr.at(1).begin();

			while (it1 != fh_temp->v_feat_ptr.at(1).end())
			{
				std::vector<double> marginal_timestamps = {marginal_time};
				(*it1).second->cleanInvalidMeasurements(marginal_timestamps);
				it1 = fh_temp->v_feat_ptr.at(1).begin();
			}
		}

		state_ptr->clones_imu.erase(marginal_time);
		state_ptr->clones_frame.at(marginal_time)->release();
		state_ptr->clones_frame.erase(marginal_time);
	}
}
    static void marginalizeSlam(std::shared_ptr<state> state_ptr, voxelHashMap &voxel_map, pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_history) {
	// display
	points_history->clear();
	// display

	int ct_marginalized = 0;
	auto it0 = state_ptr->map_points.begin();
	while (it0 != state_ptr->map_points.end())
	{
		if ((*it0).second->should_marg)
		{
			mapManagement::deleteFromVoxel(voxel_map, (*it0).second);
			stateHelper::marginalize(state_ptr, (*it0).second);

			// display
			pcl::PointXYZRGB point_temp;
			point_temp.x = (*it0).second->getPointXYZ(false)[0];
			point_temp.y = (*it0).second->getPointXYZ(false)[1];
			point_temp.z = (*it0).second->getPointXYZ(false)[2];
			point_temp.r = (*it0).second->color;
			point_temp.g = (*it0).second->color;
			point_temp.b = (*it0).second->color;
			points_history->points.push_back(point_temp);
			// display

			it0 = state_ptr->map_points.erase(it0);
			ct_marginalized++;
		}
		else
		{
			it0++;
		}
	}
}
};

class preIntegrationBase {
  public:
    preIntegrationBase(double sigma_w_, double sigma_wb_, double sigma_a_, double sigma_ab_, bool imu_avg_ = false) {
	Q_c.block(0, 0, 3, 3) = std::pow(sigma_w_, 2) * Eigen::Matrix<double, 3, 3>::Identity();
	Q_c.block(3, 3, 3, 3) = std::pow(sigma_wb_, 2) * Eigen::Matrix<double, 3, 3>::Identity();
	Q_c.block(6, 6, 3, 3) = std::pow(sigma_a_, 2) * Eigen::Matrix<double, 3, 3>::Identity();
	Q_c.block(9, 9, 3, 3) = std::pow(sigma_ab_, 2) * Eigen::Matrix<double, 3, 3>::Identity();

	imu_avg = imu_avg_;

	e_1 << 1, 0, 0;
	e_2 << 0, 1, 0;
	e_3 << 0, 0, 1;
	e_1x = quatType::skewSymmetric(e_1);
	e_2x = quatType::skewSymmetric(e_2);
	e_3x = quatType::skewSymmetric(e_3);
}
    void setLinearizationPoints(Eigen::Matrix<double, 3, 1> b_w_lin_, Eigen::Matrix<double, 3, 1> b_a_lin_,
		Eigen::Matrix<double, 4, 1> q_k_lin_ = Eigen::Matrix<double, 4, 1>::Zero(), Eigen::Matrix<double, 3, 1> grav_ = Eigen::Matrix<double, 3, 1>::Zero()) {
	b_w_lin = b_w_lin_;
	b_a_lin = b_a_lin_;
	q_k_lin = q_k_lin_;
	grav = grav_;
}
    virtual void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) = 0 {
	double delta_t = t_1 - t_0;
	dt += delta_t;

	if (delta_t == 0) return;

	Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
	Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	if (imu_avg)
	{
		w_hat += w_m_1 - b_w_lin;
		w_hat = 0.5 * w_hat;
	}

	Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;

	double w_1 = w_hat(0, 0);
	double w_2 = w_hat(1, 0);
	double w_3 = w_hat(2, 0);

	double mag_w = w_hat.norm();
	double w_dt = mag_w * delta_t;

	bool small_w = (mag_w < 0.008726646);

	double dt_2 = std::pow(delta_t, 2);
	double cos_wt = std::cos(w_dt);
	double sin_wt = std::sin(w_dt);

	Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
	Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
	Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;

	Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (std::pow(mag_w, 2.0))) * w_x_2;

	Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
	Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();

	if (imu_avg)
	{
		a_hat += a_m_1 - b_a_lin - R_tau1_k * quatType::quatToRot(q_k_lin) * grav;
		a_hat = 0.5 * a_hat;
	}
	Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);

	double f_1;
	double f_2;
	double f_3;
	double f_4;

	if (small_w)
	{
		f_1 = - (std::pow(delta_t, 3) / 3);
		f_2 = (std::pow(delta_t, 4) / 8);
		f_3 = - (std::pow(delta_t, 2) / 2);
		f_4 = (std::pow(delta_t, 3) / 6);
	}
	else
	{
		f_1 = (w_dt * cos_wt - sin_wt) / (std::pow(mag_w, 3));
		f_2 = (std::pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * std::pow(mag_w, 4));
		f_3 = - (1 - cos_wt) / std::pow(mag_w, 2);
		f_4 = (w_dt - sin_wt) / std::pow(mag_w, 3);
	}

	Eigen::Matrix<double, 3, 3> alpha_arg = ((dt_2 / 2.0) * Eigen::Matrix<double, 3, 3>::Identity() + f_1 * w_x + f_2 * w_x_2);
	Eigen::Matrix<double, 3, 3> Beta_arg = (delta_t * Eigen::Matrix<double, 3, 3>::Identity() + f_3 * w_x + f_4 * w_x_2);

	Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
	Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;

	alpha_tau += beta_tau * delta_t + H_al * a_hat;
	beta_tau += H_be * a_hat;

	Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - 0.5 * w_tx + (1.0 / 6.0) * w_tx * w_tx
		: Eigen::Matrix<double, 3, 3>::Identity() - ((1 - cos_wt) / (std::pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (std::pow(w_dt, 3.0))) * w_tx * w_tx;

	Eigen::Matrix<double, 3, 3> J_save = J_q;
	J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;

	H_a -= H_al;
	H_a += delta_t * H_b;
	H_b -= H_be;

	Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
	O_a += delta_t * O_b;
	O_a += -H_al * R_tau_k * quatType::skewSymmetric(g_k);
	O_b += -H_be * R_tau_k * quatType::skewSymmetric(g_k);

	Eigen::MatrixXd d_R_bw_1 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_1);
	Eigen::MatrixXd d_R_bw_2 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_2);
	Eigen::MatrixXd d_R_bw_3 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_3);

	double df_1_dbw_1;
	double df_1_dbw_2;
	double df_1_dbw_3;

	double df_2_dbw_1;
	double df_2_dbw_2;
	double df_2_dbw_3;

	double df_3_dbw_1;
	double df_3_dbw_2;
	double df_3_dbw_3;

	double df_4_dbw_1;
	double df_4_dbw_2;
	double df_4_dbw_3;

	if (small_w)
	{
		double df_1_dw_mag = -(std::pow(delta_t, 5) / 15);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(delta_t, 6) / 72);
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = -(std::pow(delta_t, 4) / 12);
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (std::pow(delta_t, 5) / 60);
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}
	else
	{
		double df_1_dw_mag = (std::pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / std::pow(mag_w, 5);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + std::pow(w_dt, 2) * cos_wt + 4) / (std::pow(mag_w, 6));
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (std::pow(mag_w, 4));
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (std::pow(mag_w, 5));
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}

	Eigen::Matrix<double, 3, 1> g_tau = R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	J_a += J_b * delta_t;
	J_a.block(0, 0, 3, 1) +=
		(d_R_bw_1 * alpha_arg + R_k_tau1 * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_a.block(0, 1, 3, 1) +=
		(d_R_bw_2 * alpha_arg + R_k_tau1 * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_a.block(0, 2, 3, 1) +=
		(d_R_bw_3 * alpha_arg + R_k_tau1 * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_3)) * g_tau;
	J_b.block(0, 0, 3, 1) +=
		(d_R_bw_1 * Beta_arg + R_k_tau1 * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		-H_be * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_b.block(0, 1, 3, 1) +=
		(d_R_bw_2 * Beta_arg + R_k_tau1 * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_b.block(0, 2, 3, 1) +=
		(d_R_bw_3 * Beta_arg + R_k_tau1 * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_3)) * g_tau;

	Eigen::Matrix<double, 3, 3> R_G_to_k = quatType::quatToRot(q_k_lin);
	double dt_mid = delta_t / 2.0;
	double w_dt_mid = mag_w * dt_mid;
	Eigen::Matrix<double, 3, 3> R_mid;

	R_mid = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - dt_mid * w_x + (std::pow(dt_mid, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (std::sin(w_dt_mid) / mag_w) * w_x + ((1.0 - std::cos(w_dt_mid)) / (std::pow(mag_w, 2.0))) * w_x_2;
	R_mid = R_mid * R_tau_k;

	Eigen::Matrix<double, 21, 21> F_k1 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k1.block(0, 0, 3, 3) = - w_x;
	F_k1.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k1.block(6, 0, 3, 3) = - R_tau_k.transpose() * a_x;
	F_k1.block(6, 9, 3, 3) = - R_tau_k.transpose();
	F_k1.block(6, 15, 3, 3) = - R_tau_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k1.block(6, 18, 3, 3) = - R_tau_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k1.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k1 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k1.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(6, 6, 3, 3) = - R_tau_k.transpose();
	G_k1.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_dot_k1 = F_k1;
	Eigen::Matrix<double, 21, 21> P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose();

	Eigen::Matrix<double, 21, 21> F_k2 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k2.block(0, 0, 3, 3) = - w_x;
	F_k2.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k2.block(6, 0, 3, 3) = - R_mid.transpose() * a_x;
	F_k2.block(6, 9, 3, 3) = - R_mid.transpose();
	F_k2.block(6, 15, 3, 3) = - R_mid.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k2.block(6, 18, 3, 3) = - R_mid.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k2.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k2 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k2.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(6, 6, 3, 3) = - R_mid.transpose();
	G_k2.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k2 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k2 = P_big + P_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k2 = F_k2 * Phi_k2;
	Eigen::Matrix<double, 21, 21> P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose();

	Eigen::Matrix<double, 21, 21> F_k3 = F_k2;
	Eigen::Matrix<double, 21, 12> G_k3 = G_k2;

	Eigen::Matrix<double, 21, 21> Phi_k3 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k3 = P_big + P_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k3 = F_k3 * Phi_k3;
	Eigen::Matrix<double, 21, 21> P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose();

	Eigen::Matrix<double, 21, 21> F_k4 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k4.block(0, 0, 3, 3) = - w_x;
	F_k4.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k4.block(6, 0, 3, 3) = - R_tau1_k.transpose() * a_x;
	F_k4.block(6, 9, 3, 3) = - R_tau1_k.transpose();
	F_k4.block(6, 15, 3, 3) = - R_tau1_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k4.block(6, 18, 3, 3) = - R_tau1_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k4.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k4 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k4.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(6, 6, 3, 3) = - R_tau1_k.transpose();
	G_k4.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k4 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> P_k4 = P_big + P_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> Phi_dot_k4 = F_k4 * Phi_k4;
	Eigen::Matrix<double, 21, 21> P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose();

	P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4);
	P_big = 0.5 * (P_big + P_big.transpose());

	Eigen::Matrix<double, 21, 21> Phi =
		Eigen::Matrix<double, 21, 21>::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4);

	Eigen::Matrix<double, 21, 21> B_k = Eigen::Matrix<double, 21, 21>::Identity();
	B_k.block(15, 15, 3, 3).setZero();
	B_k.block(15, 0, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	P_big = B_k * P_big * B_k.transpose();
	P_big = 0.5 * (P_big + P_big.transpose());
	Discrete_J_b = B_k * Phi * Discrete_J_b;

	P_meas = P_big.block(0, 0, 15, 15);

	if (state_transition_jacobians)
	{
		J_q = - Discrete_J_b.block(0, 3, 3, 3);
		J_a = Discrete_J_b.block(12, 3, 3, 3);
		J_b = Discrete_J_b.block(6, 3, 3, 3);
		H_a = Discrete_J_b.block(12, 9, 3, 3);
		H_b = Discrete_J_b.block(6, 9, 3, 3);
		O_a = Discrete_J_b.block(12, 18, 3, 3);
		O_b = Discrete_J_b.block(6, 18, 3, 3);
	}

	R_tau_k = R_tau1_k;
	q_tau_k = quatType::rotToQuat(R_tau_k);
}
    bool imu_avg = false;
    double dt = 0;
    Eigen::Matrix<double, 3, 1> alpha_tau = Eigen::Matrix<double, 3, 1>::Zero();
    Eigen::Matrix<double, 3, 1> beta_tau = Eigen::Matrix<double, 3, 1>::Zero();
    Eigen::Matrix<double, 4, 1> q_tau_k;
    Eigen::Matrix<double, 3, 3> R_tau_k = Eigen::Matrix<double, 3, 3>::Identity();
    Eigen::Matrix<double, 3, 3> J_q = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> J_a = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> J_b = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> H_a = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> H_b = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 1> b_w_lin;
    Eigen::Matrix<double, 3, 1> b_a_lin;
    Eigen::Matrix<double, 4, 1> q_k_lin;
    Eigen::Matrix<double, 3, 1> grav = Eigen::Matrix<double, 3, 1>::Zero();
    Eigen::Matrix<double, 12, 12> Q_c = Eigen::Matrix<double, 12, 12>::Zero();
    Eigen::Matrix<double, 15, 15> P_meas = Eigen::Matrix<double, 15, 15>::Zero();
    Eigen::Matrix<double, 3, 1> e_1;
    Eigen::Matrix<double, 3, 1> e_2;
    Eigen::Matrix<double, 3, 1> e_3;
    Eigen::Matrix<double, 3, 3> e_1x;
    Eigen::Matrix<double, 3, 3> e_2x;
    Eigen::Matrix<double, 3, 3> e_3x;
  public:
    void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) {
	double delta_t = t_1 - t_0;
	dt += delta_t;

	if (delta_t == 0) return;

	Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
	Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	if (imu_avg)
	{
		w_hat += w_m_1 - b_w_lin;
		w_hat = 0.5 * w_hat;
	}

	Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;

	double w_1 = w_hat(0, 0);
	double w_2 = w_hat(1, 0);
	double w_3 = w_hat(2, 0);

	double mag_w = w_hat.norm();
	double w_dt = mag_w * delta_t;

	bool small_w = (mag_w < 0.008726646);

	double dt_2 = std::pow(delta_t, 2);
	double cos_wt = std::cos(w_dt);
	double sin_wt = std::sin(w_dt);

	Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
	Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
	Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;

	Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (std::pow(mag_w, 2.0))) * w_x_2;

	Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
	Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();

	if (imu_avg)
	{
		a_hat += a_m_1 - b_a_lin - R_tau1_k * quatType::quatToRot(q_k_lin) * grav;
		a_hat = 0.5 * a_hat;
	}
	Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);

	double f_1;
	double f_2;
	double f_3;
	double f_4;

	if (small_w)
	{
		f_1 = - (std::pow(delta_t, 3) / 3);
		f_2 = (std::pow(delta_t, 4) / 8);
		f_3 = - (std::pow(delta_t, 2) / 2);
		f_4 = (std::pow(delta_t, 3) / 6);
	}
	else
	{
		f_1 = (w_dt * cos_wt - sin_wt) / (std::pow(mag_w, 3));
		f_2 = (std::pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * std::pow(mag_w, 4));
		f_3 = - (1 - cos_wt) / std::pow(mag_w, 2);
		f_4 = (w_dt - sin_wt) / std::pow(mag_w, 3);
	}

	Eigen::Matrix<double, 3, 3> alpha_arg = ((dt_2 / 2.0) * Eigen::Matrix<double, 3, 3>::Identity() + f_1 * w_x + f_2 * w_x_2);
	Eigen::Matrix<double, 3, 3> Beta_arg = (delta_t * Eigen::Matrix<double, 3, 3>::Identity() + f_3 * w_x + f_4 * w_x_2);

	Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
	Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;

	alpha_tau += beta_tau * delta_t + H_al * a_hat;
	beta_tau += H_be * a_hat;

	Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - 0.5 * w_tx + (1.0 / 6.0) * w_tx * w_tx
		: Eigen::Matrix<double, 3, 3>::Identity() - ((1 - cos_wt) / (std::pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (std::pow(w_dt, 3.0))) * w_tx * w_tx;

	Eigen::Matrix<double, 3, 3> J_save = J_q;
	J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;

	H_a -= H_al;
	H_a += delta_t * H_b;
	H_b -= H_be;

	Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
	O_a += delta_t * O_b;
	O_a += -H_al * R_tau_k * quatType::skewSymmetric(g_k);
	O_b += -H_be * R_tau_k * quatType::skewSymmetric(g_k);

	Eigen::MatrixXd d_R_bw_1 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_1);
	Eigen::MatrixXd d_R_bw_2 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_2);
	Eigen::MatrixXd d_R_bw_3 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_3);

	double df_1_dbw_1;
	double df_1_dbw_2;
	double df_1_dbw_3;

	double df_2_dbw_1;
	double df_2_dbw_2;
	double df_2_dbw_3;

	double df_3_dbw_1;
	double df_3_dbw_2;
	double df_3_dbw_3;

	double df_4_dbw_1;
	double df_4_dbw_2;
	double df_4_dbw_3;

	if (small_w)
	{
		double df_1_dw_mag = -(std::pow(delta_t, 5) / 15);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(delta_t, 6) / 72);
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = -(std::pow(delta_t, 4) / 12);
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (std::pow(delta_t, 5) / 60);
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}
	else
	{
		double df_1_dw_mag = (std::pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / std::pow(mag_w, 5);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + std::pow(w_dt, 2) * cos_wt + 4) / (std::pow(mag_w, 6));
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (std::pow(mag_w, 4));
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (std::pow(mag_w, 5));
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}

	Eigen::Matrix<double, 3, 1> g_tau = R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	J_a += J_b * delta_t;
	J_a.block(0, 0, 3, 1) +=
		(d_R_bw_1 * alpha_arg + R_k_tau1 * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_a.block(0, 1, 3, 1) +=
		(d_R_bw_2 * alpha_arg + R_k_tau1 * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_a.block(0, 2, 3, 1) +=
		(d_R_bw_3 * alpha_arg + R_k_tau1 * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_3)) * g_tau;
	J_b.block(0, 0, 3, 1) +=
		(d_R_bw_1 * Beta_arg + R_k_tau1 * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		-H_be * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_b.block(0, 1, 3, 1) +=
		(d_R_bw_2 * Beta_arg + R_k_tau1 * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_b.block(0, 2, 3, 1) +=
		(d_R_bw_3 * Beta_arg + R_k_tau1 * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_3)) * g_tau;

	Eigen::Matrix<double, 3, 3> R_G_to_k = quatType::quatToRot(q_k_lin);
	double dt_mid = delta_t / 2.0;
	double w_dt_mid = mag_w * dt_mid;
	Eigen::Matrix<double, 3, 3> R_mid;

	R_mid = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - dt_mid * w_x + (std::pow(dt_mid, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (std::sin(w_dt_mid) / mag_w) * w_x + ((1.0 - std::cos(w_dt_mid)) / (std::pow(mag_w, 2.0))) * w_x_2;
	R_mid = R_mid * R_tau_k;

	Eigen::Matrix<double, 21, 21> F_k1 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k1.block(0, 0, 3, 3) = - w_x;
	F_k1.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k1.block(6, 0, 3, 3) = - R_tau_k.transpose() * a_x;
	F_k1.block(6, 9, 3, 3) = - R_tau_k.transpose();
	F_k1.block(6, 15, 3, 3) = - R_tau_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k1.block(6, 18, 3, 3) = - R_tau_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k1.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k1 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k1.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(6, 6, 3, 3) = - R_tau_k.transpose();
	G_k1.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_dot_k1 = F_k1;
	Eigen::Matrix<double, 21, 21> P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose();

	Eigen::Matrix<double, 21, 21> F_k2 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k2.block(0, 0, 3, 3) = - w_x;
	F_k2.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k2.block(6, 0, 3, 3) = - R_mid.transpose() * a_x;
	F_k2.block(6, 9, 3, 3) = - R_mid.transpose();
	F_k2.block(6, 15, 3, 3) = - R_mid.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k2.block(6, 18, 3, 3) = - R_mid.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k2.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k2 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k2.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(6, 6, 3, 3) = - R_mid.transpose();
	G_k2.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k2 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k2 = P_big + P_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k2 = F_k2 * Phi_k2;
	Eigen::Matrix<double, 21, 21> P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose();

	Eigen::Matrix<double, 21, 21> F_k3 = F_k2;
	Eigen::Matrix<double, 21, 12> G_k3 = G_k2;

	Eigen::Matrix<double, 21, 21> Phi_k3 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k3 = P_big + P_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k3 = F_k3 * Phi_k3;
	Eigen::Matrix<double, 21, 21> P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose();

	Eigen::Matrix<double, 21, 21> F_k4 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k4.block(0, 0, 3, 3) = - w_x;
	F_k4.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k4.block(6, 0, 3, 3) = - R_tau1_k.transpose() * a_x;
	F_k4.block(6, 9, 3, 3) = - R_tau1_k.transpose();
	F_k4.block(6, 15, 3, 3) = - R_tau1_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k4.block(6, 18, 3, 3) = - R_tau1_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k4.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k4 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k4.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(6, 6, 3, 3) = - R_tau1_k.transpose();
	G_k4.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k4 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> P_k4 = P_big + P_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> Phi_dot_k4 = F_k4 * Phi_k4;
	Eigen::Matrix<double, 21, 21> P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose();

	P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4);
	P_big = 0.5 * (P_big + P_big.transpose());

	Eigen::Matrix<double, 21, 21> Phi =
		Eigen::Matrix<double, 21, 21>::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4);

	Eigen::Matrix<double, 21, 21> B_k = Eigen::Matrix<double, 21, 21>::Identity();
	B_k.block(15, 15, 3, 3).setZero();
	B_k.block(15, 0, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	P_big = B_k * P_big * B_k.transpose();
	P_big = 0.5 * (P_big + P_big.transpose());
	Discrete_J_b = B_k * Phi * Discrete_J_b;

	P_meas = P_big.block(0, 0, 15, 15);

	if (state_transition_jacobians)
	{
		J_q = - Discrete_J_b.block(0, 3, 3, 3);
		J_a = Discrete_J_b.block(12, 3, 3, 3);
		J_b = Discrete_J_b.block(6, 3, 3, 3);
		H_a = Discrete_J_b.block(12, 9, 3, 3);
		H_b = Discrete_J_b.block(6, 9, 3, 3);
		O_a = Discrete_J_b.block(12, 18, 3, 3);
		O_b = Discrete_J_b.block(6, 18, 3, 3);
	}

	R_tau_k = R_tau1_k;
	q_tau_k = quatType::rotToQuat(R_tau_k);
}
  private:
    Eigen::Matrix<double, 21, 21> P_big = Eigen::Matrix<double, 21, 21>::Zero();
    Eigen::Matrix<double, 21, 21> Discrete_J_b = Eigen::Matrix<double, 21, 21>::Identity();
  public:
    bool state_transition_jacobians = true;
    Eigen::Matrix<double, 3, 3> O_a = Eigen::Matrix<double, 3, 3>::Zero();
    Eigen::Matrix<double, 3, 3> O_b = Eigen::Matrix<double, 3, 3>::Zero();
    void feedImu(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
		Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) {
	double delta_t = t_1 - t_0;
	dt += delta_t;

	if (delta_t == 0) return;

	Eigen::Matrix<double, 3, 1> w_hat = w_m_0 - b_w_lin;
	Eigen::Matrix<double, 3, 1> a_hat = a_m_0 - b_a_lin - R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	if (imu_avg)
	{
		w_hat += w_m_1 - b_w_lin;
		w_hat = 0.5 * w_hat;
	}

	Eigen::Matrix<double, 3, 1> w_hat_dt = w_hat * delta_t;

	double w_1 = w_hat(0, 0);
	double w_2 = w_hat(1, 0);
	double w_3 = w_hat(2, 0);

	double mag_w = w_hat.norm();
	double w_dt = mag_w * delta_t;

	bool small_w = (mag_w < 0.008726646);

	double dt_2 = std::pow(delta_t, 2);
	double cos_wt = std::cos(w_dt);
	double sin_wt = std::sin(w_dt);

	Eigen::Matrix<double, 3, 3> w_x = quatType::skewSymmetric(w_hat);
	Eigen::Matrix<double, 3, 3> w_tx = quatType::skewSymmetric(w_hat_dt);
	Eigen::Matrix<double, 3, 3> w_x_2 = w_x * w_x;

	Eigen::Matrix<double, 3, 3> R_tau1_tau2 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - delta_t * w_x + (std::pow(delta_t, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (std::pow(mag_w, 2.0))) * w_x_2;

	Eigen::Matrix<double, 3, 3> R_tau1_k = R_tau1_tau2 * R_tau_k;
	Eigen::Matrix<double, 3, 3> R_k_tau1 = R_tau1_k.transpose();

	if (imu_avg)
	{
		a_hat += a_m_1 - b_a_lin - R_tau1_k * quatType::quatToRot(q_k_lin) * grav;
		a_hat = 0.5 * a_hat;
	}
	Eigen::Matrix<double, 3, 3> a_x = quatType::skewSymmetric(a_hat);

	double f_1;
	double f_2;
	double f_3;
	double f_4;

	if (small_w)
	{
		f_1 = - (std::pow(delta_t, 3) / 3);
		f_2 = (std::pow(delta_t, 4) / 8);
		f_3 = - (std::pow(delta_t, 2) / 2);
		f_4 = (std::pow(delta_t, 3) / 6);
	}
	else
	{
		f_1 = (w_dt * cos_wt - sin_wt) / (std::pow(mag_w, 3));
		f_2 = (std::pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * std::pow(mag_w, 4));
		f_3 = - (1 - cos_wt) / std::pow(mag_w, 2);
		f_4 = (w_dt - sin_wt) / std::pow(mag_w, 3);
	}

	Eigen::Matrix<double, 3, 3> alpha_arg = ((dt_2 / 2.0) * Eigen::Matrix<double, 3, 3>::Identity() + f_1 * w_x + f_2 * w_x_2);
	Eigen::Matrix<double, 3, 3> Beta_arg = (delta_t * Eigen::Matrix<double, 3, 3>::Identity() + f_3 * w_x + f_4 * w_x_2);

	Eigen::Matrix<double, 3, 3> H_al = R_k_tau1 * alpha_arg;
	Eigen::Matrix<double, 3, 3> H_be = R_k_tau1 * Beta_arg;

	alpha_tau += beta_tau * delta_t + H_al * a_hat;
	beta_tau += H_be * a_hat;

	Eigen::Matrix<double, 3, 3> J_r_tau1 = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - 0.5 * w_tx + (1.0 / 6.0) * w_tx * w_tx
		: Eigen::Matrix<double, 3, 3>::Identity() - ((1 - cos_wt) / (std::pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (std::pow(w_dt, 3.0))) * w_tx * w_tx;

	Eigen::Matrix<double, 3, 3> J_save = J_q;
	J_q = R_tau1_tau2 * J_q + J_r_tau1 * delta_t;

	H_a -= H_al;
	H_a += delta_t * H_b;
	H_b -= H_be;

	Eigen::Matrix<double, 3, 1> g_k = quatType::quatToRot(q_k_lin) * grav;
	O_a += delta_t * O_b;
	O_a += -H_al * R_tau_k * quatType::skewSymmetric(g_k);
	O_b += -H_be * R_tau_k * quatType::skewSymmetric(g_k);

	Eigen::MatrixXd d_R_bw_1 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_1);
	Eigen::MatrixXd d_R_bw_2 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_2);
	Eigen::MatrixXd d_R_bw_3 = - R_k_tau1 * quatType::skewSymmetric(J_q * e_3);

	double df_1_dbw_1;
	double df_1_dbw_2;
	double df_1_dbw_3;

	double df_2_dbw_1;
	double df_2_dbw_2;
	double df_2_dbw_3;

	double df_3_dbw_1;
	double df_3_dbw_2;
	double df_3_dbw_3;

	double df_4_dbw_1;
	double df_4_dbw_2;
	double df_4_dbw_3;

	if (small_w)
	{
		double df_1_dw_mag = -(std::pow(delta_t, 5) / 15);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(delta_t, 6) / 72);
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = -(std::pow(delta_t, 4) / 12);
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (std::pow(delta_t, 5) / 60);
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}
	else
	{
		double df_1_dw_mag = (std::pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / std::pow(mag_w, 5);
		df_1_dbw_1 = w_1 * df_1_dw_mag;
		df_1_dbw_2 = w_2 * df_1_dw_mag;
		df_1_dbw_3 = w_3 * df_1_dw_mag;

		double df_2_dw_mag = (std::pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + std::pow(w_dt, 2) * cos_wt + 4) / (std::pow(mag_w, 6));
		df_2_dbw_1 = w_1 * df_2_dw_mag;
		df_2_dbw_2 = w_2 * df_2_dw_mag;
		df_2_dbw_3 = w_3 * df_2_dw_mag;

		double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (std::pow(mag_w, 4));
		df_3_dbw_1 = w_1 * df_3_dw_mag;
		df_3_dbw_2 = w_2 * df_3_dw_mag;
		df_3_dbw_3 = w_3 * df_3_dw_mag;

		double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (std::pow(mag_w, 5));
		df_4_dbw_1 = w_1 * df_4_dw_mag;
		df_4_dbw_2 = w_2 * df_4_dw_mag;
		df_4_dbw_3 = w_3 * df_4_dw_mag;
	}

	Eigen::Matrix<double, 3, 1> g_tau = R_tau_k * quatType::quatToRot(q_k_lin) * grav;

	J_a += J_b * delta_t;
	J_a.block(0, 0, 3, 1) +=
		(d_R_bw_1 * alpha_arg + R_k_tau1 * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_a.block(0, 1, 3, 1) +=
		(d_R_bw_2 * alpha_arg + R_k_tau1 * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_a.block(0, 2, 3, 1) +=
		(d_R_bw_3 * alpha_arg + R_k_tau1 * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_al * quatType::skewSymmetric((J_save * e_3)) * g_tau;
	J_b.block(0, 0, 3, 1) +=
		(d_R_bw_1 * Beta_arg + R_k_tau1 * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat -
		-H_be * quatType::skewSymmetric((J_save * e_1)) * g_tau;
	J_b.block(0, 1, 3, 1) +=
		(d_R_bw_2 * Beta_arg + R_k_tau1 * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_2)) * g_tau;
	J_b.block(0, 2, 3, 1) +=
		(d_R_bw_3 * Beta_arg + R_k_tau1 * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat -
		H_be * quatType::skewSymmetric((J_save * e_3)) * g_tau;

	Eigen::Matrix<double, 3, 3> R_G_to_k = quatType::quatToRot(q_k_lin);
	double dt_mid = delta_t / 2.0;
	double w_dt_mid = mag_w * dt_mid;
	Eigen::Matrix<double, 3, 3> R_mid;

	R_mid = small_w ? Eigen::Matrix<double, 3, 3>::Identity() - dt_mid * w_x + (std::pow(dt_mid, 2) / 2) * w_x_2
		: Eigen::Matrix<double, 3, 3>::Identity() - (std::sin(w_dt_mid) / mag_w) * w_x + ((1.0 - std::cos(w_dt_mid)) / (std::pow(mag_w, 2.0))) * w_x_2;
	R_mid = R_mid * R_tau_k;

	Eigen::Matrix<double, 21, 21> F_k1 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k1.block(0, 0, 3, 3) = - w_x;
	F_k1.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k1.block(6, 0, 3, 3) = - R_tau_k.transpose() * a_x;
	F_k1.block(6, 9, 3, 3) = - R_tau_k.transpose();
	F_k1.block(6, 15, 3, 3) = - R_tau_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k1.block(6, 18, 3, 3) = - R_tau_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k1.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k1 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k1.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k1.block(6, 6, 3, 3) = - R_tau_k.transpose();
	G_k1.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_dot_k1 = F_k1;
	Eigen::Matrix<double, 21, 21> P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose();

	Eigen::Matrix<double, 21, 21> F_k2 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k2.block(0, 0, 3, 3) = - w_x;
	F_k2.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k2.block(6, 0, 3, 3) = - R_mid.transpose() * a_x;
	F_k2.block(6, 9, 3, 3) = - R_mid.transpose();
	F_k2.block(6, 15, 3, 3) = - R_mid.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k2.block(6, 18, 3, 3) = - R_mid.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k2.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k2 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k2.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k2.block(6, 6, 3, 3) = - R_mid.transpose();
	G_k2.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k2 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k2 = P_big + P_dot_k1 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k2 = F_k2 * Phi_k2;
	Eigen::Matrix<double, 21, 21> P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose();

	Eigen::Matrix<double, 21, 21> F_k3 = F_k2;
	Eigen::Matrix<double, 21, 12> G_k3 = G_k2;

	Eigen::Matrix<double, 21, 21> Phi_k3 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> P_k3 = P_big + P_dot_k2 * dt_mid;
	Eigen::Matrix<double, 21, 21> Phi_dot_k3 = F_k3 * Phi_k3;
	Eigen::Matrix<double, 21, 21> P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose();

	Eigen::Matrix<double, 21, 21> F_k4 = Eigen::Matrix<double, 21, 21>::Zero();
	F_k4.block(0, 0, 3, 3) = - w_x;
	F_k4.block(0, 3, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	F_k4.block(6, 0, 3, 3) = - R_tau1_k.transpose() * a_x;
	F_k4.block(6, 9, 3, 3) = - R_tau1_k.transpose();
	F_k4.block(6, 15, 3, 3) = - R_tau1_k.transpose() * quatType::skewSymmetric(R_tau_k * R_G_to_k * grav);
	F_k4.block(6, 18, 3, 3) = - R_tau1_k.transpose() * R_tau_k * quatType::skewSymmetric(R_G_to_k * grav);
	F_k4.block(12, 6, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 12> G_k4 = Eigen::Matrix<double, 21, 12>::Zero();
	G_k4.block(0, 0, 3, 3) = - Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(3, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
	G_k4.block(6, 6, 3, 3) = - R_tau1_k.transpose();
	G_k4.block(9, 9, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	Eigen::Matrix<double, 21, 21> Phi_k4 = Eigen::Matrix<double, 21, 21>::Identity() + Phi_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> P_k4 = P_big + P_dot_k3 * delta_t;
	Eigen::Matrix<double, 21, 21> Phi_dot_k4 = F_k4 * Phi_k4;
	Eigen::Matrix<double, 21, 21> P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose();

	P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4);
	P_big = 0.5 * (P_big + P_big.transpose());

	Eigen::Matrix<double, 21, 21> Phi =
		Eigen::Matrix<double, 21, 21>::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4);

	Eigen::Matrix<double, 21, 21> B_k = Eigen::Matrix<double, 21, 21>::Identity();
	B_k.block(15, 15, 3, 3).setZero();
	B_k.block(15, 0, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

	P_big = B_k * P_big * B_k.transpose();
	P_big = 0.5 * (P_big + P_big.transpose());
	Discrete_J_b = B_k * Phi * Discrete_J_b;

	P_meas = P_big.block(0, 0, 15, 15);

	if (state_transition_jacobians)
	{
		J_q = - Discrete_J_b.block(0, 3, 3, 3);
		J_a = Discrete_J_b.block(12, 3, 3, 3);
		J_b = Discrete_J_b.block(6, 3, 3, 3);
		H_a = Discrete_J_b.block(12, 9, 3, 3);
		H_b = Discrete_J_b.block(6, 9, 3, 3);
		O_a = Discrete_J_b.block(12, 18, 3, 3);
		O_b = Discrete_J_b.block(6, 18, 3, 3);
	}

	R_tau_k = R_tau1_k;
	q_tau_k = quatType::rotToQuat(R_tau_k);
}
  public:
    bool Plus(const double *x, const double *delta, double *x_plus_delta) const override {
	Eigen::Map<const Eigen::Vector4d> q(x);

	Eigen::Map<const Eigen::Vector3d> d_th(delta);
	Eigen::Matrix<double, 4, 1> d_q;
	double theta = d_th.norm();

	if (theta < 1e-8)
		d_q << 0.5 * d_th, 1.0;
	else
	{
		d_q.block(0, 0, 3, 1) = (d_th / theta) * std::sin(theta / 2);
		d_q(3, 0) = std::cos(theta / 2);
	}
	d_q = quatType::quatNorm(d_q);

	Eigen::Map<Eigen::Vector4d> q_plus(x_plus_delta);
	q_plus = quatType::quatMultiply(d_q, q);

	return true;
}
    bool ComputeJacobian(const double *x, double *jacobian) const override {
	Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> j(jacobian);
	j.topRows<3>().setIdentity();
	j.bottomRows<1>().setZero();

	return true;
}
  public:
    Eigen::MatrixXd x_lin;
    std::vector<std::string> x_type;
    Eigen::MatrixXd sqrt_I;
    Eigen::MatrixXd b;
    factorGenericPrior(const Eigen::MatrixXd &x_lin_, const std::vector<std::string> &x_type_, const Eigen::MatrixXd &prior_Info,
		const Eigen::MatrixXd &prior_grad) {
	int state_size = 0;
	int state_error_size = 0;

	for (auto const &str : x_type_)
	{
		if (str == "quat")
		{
			state_size += 4;
			state_error_size += 3;
		}
		else if (str == "quat_yaw")
		{
			state_size += 4;
			state_error_size += 1;
		}
		else if (str == "vec1")
		{
			state_size += 1;
			state_error_size += 1;
		}
		else if (str == "vec3")
		{
			state_size += 3;
			state_error_size += 3;
		}
		else if (str == "vec8")
		{
			state_size += 8;
			state_error_size += 8;
		}
		else
		{
			std::cerr << "type - " << str << " not implemented in prior" << std::endl;
			std::exit(EXIT_FAILURE);
		}
	}

	assert(x_lin.rows() == state_size);
	assert(x_lin.cols() == 1);
	assert(prior_Info.rows() == state_error_size);
	assert(prior_Info.cols() == state_error_size);
	assert(prior_grad.rows() == state_error_size);
	assert(prior_grad.cols() == 1);

	Eigen::LLT<Eigen::MatrixXd> llt_of_I(prior_Info);
	sqrt_I = llt_of_I.matrixL().transpose();
	Eigen::MatrixXd I = Eigen::MatrixXd::Identity(prior_Info.rows(), prior_Info.rows());
	b = sqrt_I.triangularView<Eigen::Upper>().solve(I) * prior_grad;

	if (std::isnan(prior_Info.norm()) || std::isnan(sqrt_I.norm()) || std::isnan(b.norm()))
	{
		std::cerr << "prior_Info - " << std::endl << prior_Info << std::endl << std::endl;
		std::cerr << "prior_Info_inv - " << std::endl << prior_Info.inverse() << std::endl << std::endl;
		std::cerr << "b - " << std::endl << b << std::endl << std::endl;
		std::exit(EXIT_FAILURE);
	}

	set_num_residuals(state_error_size);

	for (auto const &str : x_type_)
	{
		if (str == "quat")
			mutable_parameter_block_sizes()->push_back(4);
		if (str == "quat_yaw")
			mutable_parameter_block_sizes()->push_back(4);
		if (str == "vec1")
			mutable_parameter_block_sizes()->push_back(1);
		if (str == "vec3")
			mutable_parameter_block_sizes()->push_back(3);
		if (str == "vec8")
			mutable_parameter_block_sizes()->push_back(8);
	}
}
    bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
	Eigen::Vector4d q_GtoIi = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
	Eigen::Matrix3d R_GtoIi = quatType::quatToRot(q_GtoIi);
	Eigen::Vector3d p_IiinG = Eigen::Map<const Eigen::Vector3d>(parameters[1]);
	Eigen::Vector3d p_FinG = Eigen::Map<const Eigen::Vector3d>(parameters[2]);
	Eigen::Vector4d q_ItoC = Eigen::Map<const Eigen::Vector4d>(parameters[3]);
	Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);
	Eigen::Vector3d p_IinC = Eigen::Map<const Eigen::Vector3d>(parameters[4]);

	Eigen::Matrix<double, 8, 1> camera_vals = Eigen::Map<const Eigen::Matrix<double, 8, 1>>(parameters[5]);

	Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
	Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;

	Eigen::Vector2d uv_norm;
	uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

	Eigen::Matrix<double, 2, 2> sqrt_Q_gate = gate * sqrt_Q;

	Eigen::Vector2d uv_dist;
	Eigen::MatrixXd H_dz_dzn, H_dz_dzeta;

	if (is_fisheye)
	{
		cameraEqui cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}
	else
	{
		cameraRadtan cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}

	Eigen::Vector2d res = uv_dist - uv_meas;
	res = sqrt_Q_gate * res;
	residuals[0] = res(0);
	residuals[1] = res(1);

	if (jacobians)
	{
		Eigen::MatrixXd H_dzn_dpfc = Eigen::MatrixXd::Zero(2, 3);
		H_dzn_dpfc << 1.0 / p_FinCi(2), 0, -p_FinCi(0) / std::pow(p_FinCi(2), 2), 0, 1.0 / p_FinCi(2), -p_FinCi(1) / std::pow(p_FinCi(2), 2);
		Eigen::MatrixXd H_dz_dpfc = H_dz_dzn * H_dzn_dpfc;

		if (jacobians[0])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[0]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * quatType::skewSymmetric(p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[1]) {
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[1]);
			jacobian.block(0, 0, 2, 3) = -H_dz_dpfc * R_ItoC * R_GtoIi;
		}

		if (jacobians[2])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[2]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * R_GtoIi;
		}

		if (jacobians[3])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[3]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * quatType::skewSymmetric(R_ItoC * p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[4])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[4]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc;
		}

		if (jacobians[5])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 8, Eigen::RowMajor>> jacobian(jacobians[5]);
			jacobian.block(0, 0, 2, 8) = H_dz_dzeta;
		}
	}

	return true;
}
  public:
    Eigen::Vector3d alpha;
    Eigen::Vector3d beta;
    Eigen::Vector4d q_breve;
    double dt;
    Eigen::Vector3d b_w_lin_save;
    Eigen::Vector3d b_a_lin_save;
    Eigen::Matrix3d J_q;
    Eigen::Matrix3d J_a;
    Eigen::Matrix3d J_b;
    Eigen::Matrix3d H_a;
    Eigen::Matrix3d H_b;
    Eigen::Matrix<double, 15, 15> sqrt_I_save;
    Eigen::Vector3d grav_save;
    factorImuPreIntegrationV1(double deltatime, Eigen::Vector3d &grav, Eigen::Vector3d &alpha, Eigen::Vector3d &beta, Eigen::Vector4d &q_KtoK1,
		Eigen::Vector3d &ba_lin, Eigen::Vector3d &bg_lin, Eigen::Matrix3d &J_q, Eigen::Matrix3d &J_beta, Eigen::Matrix3d &J_alpha,
		Eigen::Matrix3d &H_beta, Eigen::Matrix3d &H_alpha, Eigen::Matrix<double, 15, 15> &covariance) {
	this->alpha = alpha;
	this->beta = beta;
	this->q_breve = q_KtoK1;
	this->dt = deltatime;
	this->grav_save = grav;

	this->b_a_lin_save = ba_lin;
	this->b_w_lin_save = bg_lin;

	this->J_q = J_q;
	this->J_a = J_alpha;
	this->J_b = J_beta;
	this->H_a = H_alpha;
	this->H_b = H_beta;

	Eigen::MatrixXd I = Eigen::MatrixXd::Identity(covariance.rows(), covariance.rows());
	Eigen::MatrixXd information = covariance.llt().solve(I);

	if (std::isnan(information.norm()))
	{
		std::cerr << "P - " << std::endl << covariance << std::endl << std::endl;
		std::cerr << "Pinv - " << std::endl << covariance.inverse() << std::endl << std::endl;
		std::exit(EXIT_FAILURE);
	}

	Eigen::LLT<Eigen::MatrixXd> llt_of_I(information);
	sqrt_I_save = llt_of_I.matrixL().transpose();

	set_num_residuals(15);
	mutable_parameter_block_sizes()->push_back(4);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(4);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
}
    bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
	Eigen::Vector4d q_GtoIi = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
	Eigen::Matrix3d R_GtoIi = quatType::quatToRot(q_GtoIi);
	Eigen::Vector3d p_IiinG = Eigen::Map<const Eigen::Vector3d>(parameters[1]);
	Eigen::Vector3d p_FinG = Eigen::Map<const Eigen::Vector3d>(parameters[2]);
	Eigen::Vector4d q_ItoC = Eigen::Map<const Eigen::Vector4d>(parameters[3]);
	Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);
	Eigen::Vector3d p_IinC = Eigen::Map<const Eigen::Vector3d>(parameters[4]);

	Eigen::Matrix<double, 8, 1> camera_vals = Eigen::Map<const Eigen::Matrix<double, 8, 1>>(parameters[5]);

	Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
	Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;

	Eigen::Vector2d uv_norm;
	uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

	Eigen::Matrix<double, 2, 2> sqrt_Q_gate = gate * sqrt_Q;

	Eigen::Vector2d uv_dist;
	Eigen::MatrixXd H_dz_dzn, H_dz_dzeta;

	if (is_fisheye)
	{
		cameraEqui cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}
	else
	{
		cameraRadtan cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}

	Eigen::Vector2d res = uv_dist - uv_meas;
	res = sqrt_Q_gate * res;
	residuals[0] = res(0);
	residuals[1] = res(1);

	if (jacobians)
	{
		Eigen::MatrixXd H_dzn_dpfc = Eigen::MatrixXd::Zero(2, 3);
		H_dzn_dpfc << 1.0 / p_FinCi(2), 0, -p_FinCi(0) / std::pow(p_FinCi(2), 2), 0, 1.0 / p_FinCi(2), -p_FinCi(1) / std::pow(p_FinCi(2), 2);
		Eigen::MatrixXd H_dz_dpfc = H_dz_dzn * H_dzn_dpfc;

		if (jacobians[0])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[0]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * quatType::skewSymmetric(p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[1]) {
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[1]);
			jacobian.block(0, 0, 2, 3) = -H_dz_dpfc * R_ItoC * R_GtoIi;
		}

		if (jacobians[2])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[2]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * R_GtoIi;
		}

		if (jacobians[3])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[3]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * quatType::skewSymmetric(R_ItoC * p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[4])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[4]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc;
		}

		if (jacobians[5])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 8, Eigen::RowMajor>> jacobian(jacobians[5]);
			jacobian.block(0, 0, 2, 8) = H_dz_dzeta;
		}
	}

	return true;
}
  public:
    Eigen::Vector2d uv_meas;
    double pix_sigma = 1.0;
    Eigen::Matrix<double, 2, 2> sqrt_Q;
    bool is_fisheye = false;
    double gate = 1.0;
    factorImageReprojCalib(const Eigen::Vector2d &uv_meas_, double pix_sigma_, bool is_fisheye_) {
	sqrt_Q = Eigen::Matrix<double, 2, 2>::Identity();
	sqrt_Q(0, 0) *= 1.0 / pix_sigma;
	sqrt_Q(1, 1) *= 1.0 / pix_sigma;

	set_num_residuals(2);
	mutable_parameter_block_sizes()->push_back(4);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(4);
	mutable_parameter_block_sizes()->push_back(3);
	mutable_parameter_block_sizes()->push_back(8);
}
    bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override {
	Eigen::Vector4d q_GtoIi = Eigen::Map<const Eigen::Vector4d>(parameters[0]);
	Eigen::Matrix3d R_GtoIi = quatType::quatToRot(q_GtoIi);
	Eigen::Vector3d p_IiinG = Eigen::Map<const Eigen::Vector3d>(parameters[1]);
	Eigen::Vector3d p_FinG = Eigen::Map<const Eigen::Vector3d>(parameters[2]);
	Eigen::Vector4d q_ItoC = Eigen::Map<const Eigen::Vector4d>(parameters[3]);
	Eigen::Matrix3d R_ItoC = quatType::quatToRot(q_ItoC);
	Eigen::Vector3d p_IinC = Eigen::Map<const Eigen::Vector3d>(parameters[4]);

	Eigen::Matrix<double, 8, 1> camera_vals = Eigen::Map<const Eigen::Matrix<double, 8, 1>>(parameters[5]);

	Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
	Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;

	Eigen::Vector2d uv_norm;
	uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

	Eigen::Matrix<double, 2, 2> sqrt_Q_gate = gate * sqrt_Q;

	Eigen::Vector2d uv_dist;
	Eigen::MatrixXd H_dz_dzn, H_dz_dzeta;

	if (is_fisheye)
	{
		cameraEqui cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}
	else
	{
		cameraRadtan cam(0, 0);
		cam.setValue(camera_vals);
		uv_dist = cam.distortD(uv_norm);
		if (jacobians)
		{
			cam.computeDistortJacobian(uv_norm, H_dz_dzn, H_dz_dzeta);
			H_dz_dzn = sqrt_Q_gate * H_dz_dzn;
			H_dz_dzeta = sqrt_Q_gate * H_dz_dzeta;
		}
	}

	Eigen::Vector2d res = uv_dist - uv_meas;
	res = sqrt_Q_gate * res;
	residuals[0] = res(0);
	residuals[1] = res(1);

	if (jacobians)
	{
		Eigen::MatrixXd H_dzn_dpfc = Eigen::MatrixXd::Zero(2, 3);
		H_dzn_dpfc << 1.0 / p_FinCi(2), 0, -p_FinCi(0) / std::pow(p_FinCi(2), 2), 0, 1.0 / p_FinCi(2), -p_FinCi(1) / std::pow(p_FinCi(2), 2);
		Eigen::MatrixXd H_dz_dpfc = H_dz_dzn * H_dzn_dpfc;

		if (jacobians[0])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[0]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * quatType::skewSymmetric(p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[1]) {
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[1]);
			jacobian.block(0, 0, 2, 3) = -H_dz_dpfc * R_ItoC * R_GtoIi;
		}

		if (jacobians[2])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[2]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * R_GtoIi;
		}

		if (jacobians[3])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian(jacobians[3]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc * quatType::skewSymmetric(R_ItoC * p_FinIi);
			jacobian.block(0, 3, 2, 1).setZero();
		}

		if (jacobians[4])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian(jacobians[4]);
			jacobian.block(0, 0, 2, 3) = H_dz_dpfc;
		}

		if (jacobians[5])
		{
			Eigen::Map<Eigen::Matrix<double, 2, 8, Eigen::RowMajor>> jacobian(jacobians[5]);
			jacobian.block(0, 0, 2, 8) = H_dz_dzeta;
		}
	}

	return true;
}
};

class voxelStereoVio {
  private:
    void processImu(imuData &imu_data) {
    double oldest_time = state_ptr->margtimestep();

    if (oldest_time > state_ptr->timestamp)
        oldest_time = -1;

    if (!initial_flag)
        oldest_time = imu_data.timestamp - odometry_options.init_options.init_window_time + state_ptr->calib_dt_imu_cam->value()(0) - 0.10;

    propagator_ptr->feedImu(imu_data, oldest_time);

    if (!initial_flag)
        initializer_ptr->feedImu(imu_data, oldest_time);
}
    void processImage(cameraData &image_measurements_const) {
    rT1 = boost::posix_time::microsec_clock::local_time();

    assert(!image_measurements_const.camera_ids.empty());
    assert(image_measurements_const.camera_ids.size() == image_measurements_const.images.size());

    for (size_t i = 0; i < image_measurements_const.camera_ids.size() - 1; i++)
        assert(image_measurements_const.camera_ids.at(i) != image_measurements_const.camera_ids.at(i + 1));

    // time test
    boost::posix_time::ptime rT_begin7 = boost::posix_time::microsec_clock::local_time();
    // time test

    cameraData image_measurements = image_measurements_const;

    for (size_t i = 0; i < image_measurements.camera_ids.size() && odometry_options.downsample_cameras; i++)
    {
        cv::Mat img = image_measurements.images.at(i);
        cv::Mat mask = image_measurements.masks.at(i);
        cv::Mat img_temp, mask_temp;
        cv::pyrDown(img, img_temp, cv::Size(img.cols / 2.0, img.rows / 2.0));
        image_measurements.images.at(i) = img_temp;
        cv::pyrDown(mask, mask_temp, cv::Size(mask.cols / 2.0, mask.rows / 2.0));
        image_measurements.masks.at(i) = mask_temp;
    }

    std::shared_ptr<frame> fh = std::make_shared<frame>();
    fh->makeImageLeft(image_measurements.images.at(0).data, image_measurements.masks.at(0).data, gammaPixel_ptr, image_measurements.timestamp);
    fh->makeImageRight(image_measurements.images.at(1).data, image_measurements.masks.at(1).data, gammaPixel_ptr, image_measurements.timestamp);
    fh->frame_id = frame_count;

    // time test
    boost::posix_time::ptime rT_end7 = boost::posix_time::microsec_clock::local_time();
    double time_cost = (rT_end7 - rT_begin7).total_microseconds() * 1e-6;
    sum_time_7 = sum_time_7 * n_time_7 + time_cost;
    n_time_7++;
    sum_time_7 = sum_time_7 / n_time_7;
    std::cout << std::fixed << std::setprecision(6) << "[image processing] time cost = " << sum_time_7 << std::endl;
    // time test

    newest_fh = fh;

    // time test
    boost::posix_time::ptime rT_begin1 = boost::posix_time::microsec_clock::local_time();
    // time test

    featureTracker->feedNewImage(image_measurements, fh);

    // time test
    boost::posix_time::ptime rT_end1 = boost::posix_time::microsec_clock::local_time();
    time_cost = (rT_end1 - rT_begin1).total_microseconds() * 1e-6;
    sum_time_1 = sum_time_1 * n_time_1 + time_cost;
    n_time_1++;
    sum_time_1 = sum_time_1 / n_time_1;
    std::cout << std::fixed << std::setprecision(6) << "[feature extraction and matching] time cost = " << sum_time_1 << std::endl;
    // time test

    rT2 = boost::posix_time::microsec_clock::local_time();

    if (!initial_flag)
    {
        initial_flag = tryToInitialize(image_measurements);

        if (!initial_flag)
        {
            double time_track = (rT2 - rT1).total_microseconds() * 1e-6;
            std::cout << std::fixed << "[processImage]: " << time_track << " seconds for tracking" << std::endl;
            return;
        }
    }

    if (featureTracker->getFeatureDatabase()->featureCheckParallax(frame_count))
        marginalize_status = MarginalizeStatus::OLD;
    else
    {
        if (odometry_options.use_keyframe)
            marginalize_status = MarginalizeStatus::NEW;
        else
            marginalize_status = MarginalizeStatus::OLD;
    }

    std::cout << "marginalize_status = " << marginalize_status << std::endl;

    featureUpdate(image_measurements);

    frame_count++;
}
    void featureUpdate(cameraData &image_measurements) {
    if (state_ptr->timestamp > image_measurements.timestamp)
    {
        std::cout << std::fixed << "Image received out of order, unable to do anything (prop dt = " << image_measurements.timestamp - state_ptr->timestamp << ")" << std::endl;
        return;
    }

    if (state_ptr->timestamp != image_measurements.timestamp)
    {
        propagator_ptr->propagateAndClone(state_ptr, image_measurements.timestamp);

        if (state_ptr->clones_frame.find(image_measurements.timestamp) == state_ptr->clones_frame.end())
            state_ptr->clones_frame[image_measurements.timestamp] = newest_fh;

        assert(state_ptr->clones_imu.size() == state_ptr->clones_frame.size());
    }

    rT3 = boost::posix_time::microsec_clock::local_time();

    if ((int)state_ptr->clones_imu.size() < std::min(state_ptr->options.max_clone_size, 5))
    {
        std::cout << "Waiting for enough clone states (" << (int)state_ptr->clones_imu.size() << " of "
            << std::min(state_ptr->options.max_clone_size, 5) << ")" << std::endl;
        return;
    }

    if (state_ptr->timestamp > image_measurements.timestamp)
    {
        std::cout << "Msckf unable to propagate the state forward in time" << std::endl;
        std::cout << std::fixed << "It has been " << image_measurements.timestamp - state_ptr->timestamp << " since last time we propagated" << std::endl;
        return;
    }

    std::vector<std::shared_ptr<feature>> features_lost, features_marg, features_slam;
    features_lost = featureTracker->getFeatureDatabase()->getOldFeatures(state_ptr->timestamp, false, true);

    if ((int)state_ptr->clones_imu.size() > state_ptr->options.max_clone_size || (int)state_ptr->clones_imu.size() > 5)
    {
        features_marg = featureTracker->getFeatureDatabase()->getFeatures(state_ptr->margtimestep(), false, true);
    }

    auto it_1 = features_lost.begin();
    while (it_1 != features_lost.end())
    {
        bool found_current_cam_id = false;
        for (const auto &cam_uv_pair : (*it_1)->uvs)
        {
            if (std::find(image_measurements.camera_ids.begin(), image_measurements.camera_ids.end(), cam_uv_pair.first)
                != image_measurements.camera_ids.end())
            {
                found_current_cam_id = true;
                break;
            }
        }

        if (found_current_cam_id)
        {
            it_1++;
        }
        else
        {
            it_1 = features_lost.erase(it_1);
        }
    }

    it_1 = features_lost.begin();
    while (it_1 != features_lost.end())
    {
        if (std::find(features_marg.begin(), features_marg.end(), (*it_1)) != features_marg.end())
            it_1 = features_lost.erase(it_1);
        else
            it_1++;
    }

    std::vector<std::shared_ptr<feature>> features_maxtracks;
    auto it_2 = features_marg.begin();

    while (it_2 != features_marg.end())
    {
        bool reached_max = false;

        for (const auto &cams : (*it_2)->timestamps)
        {
            if ((int)cams.second.size() > state_ptr->options.max_clone_size)
            {
                reached_max = true;
                break;
            }
        }

        if (reached_max)
        {
            features_maxtracks.push_back(*it_2);
            it_2 = features_marg.erase(it_2);
        }
        else
        {
            it_2++;
        }
    }

    if (state_ptr->options.max_slam_features > 0 && image_measurements.timestamp - startup_time >= odometry_options.dt_slam_delay &&
        (int)state_ptr->map_points.size() < state_ptr->options.max_slam_features)
    {
        int amount_to_add = (state_ptr->options.max_slam_features) - (int)state_ptr->map_points.size();
        int valid_amount = (amount_to_add > (int)features_maxtracks.size()) ? (int)features_maxtracks.size() : amount_to_add;

        if (valid_amount > 0)
        {
            features_slam.insert(features_slam.end(), features_maxtracks.end() - valid_amount, features_maxtracks.end());
            features_maxtracks.erase(features_maxtracks.end() - valid_amount, features_maxtracks.end());
        }
    }

    for (int i = 0; i < recent_voxels.size(); i++)
    {
        short kx = recent_voxels[i][0];
        short ky = recent_voxels[i][1];
        short kz = recent_voxels[i][2];

        voxelHashMap::iterator search = voxel_map.find(voxel(kx, ky, kz));
        auto &voxel_block = (search.value());

        if (odometry_options.use_all_points)
        {
            for (int j = 0; j < voxel_block.points.size(); j++)
            {
                std::shared_ptr<mapPoint> map_point_ptr = voxel_block.points[j];

                std::shared_ptr<feature> feature = featureTracker->getFeatureDatabase()->getFeature(map_point_ptr->feature_id);
                if (feature != nullptr)
                    features_slam.push_back(feature);

                assert(map_point_ptr->unique_camera_id != -1);

                bool current_host_cam = std::find(image_measurements.camera_ids.begin(), image_measurements.camera_ids.end(), map_point_ptr->unique_camera_id) != image_measurements.camera_ids.end();

                if (feature == nullptr && current_host_cam)
                    map_point_ptr->should_marg = true;

                if (map_point_ptr->update_fail_count > 1)
                    map_point_ptr->should_marg = true;
            }
        }
        else
        {
            std::shared_ptr<mapPoint> map_point_ptr = voxel_block.points.front();

            std::shared_ptr<feature> feature = featureTracker->getFeatureDatabase()->getFeature(map_point_ptr->feature_id);
            if (feature != nullptr)
                features_slam.push_back(feature);

            assert(map_point_ptr->unique_camera_id != -1);

            bool current_host_cam = std::find(image_measurements.camera_ids.begin(), image_measurements.camera_ids.end(), map_point_ptr->unique_camera_id) != image_measurements.camera_ids.end();

            if (feature == nullptr && current_host_cam)
                map_point_ptr->should_marg = true;

            if (map_point_ptr->update_fail_count > 1)
                map_point_ptr->should_marg = true;
        }
    }

    // time test
    boost::posix_time::ptime rT_begin6 = boost::posix_time::microsec_clock::local_time();
    // time test

    stateHelper::marginalizeSlam(state_ptr, voxel_map, points_history);

    // time test
    boost::posix_time::ptime rT_end6 = boost::posix_time::microsec_clock::local_time();
    double time_cost = (rT_end6 - rT_begin6).total_microseconds() * 1e-6;
    sum_time_6 = sum_time_6 * n_time_6 + time_cost;
    n_time_6++;
    sum_time_6 = sum_time_6 / n_time_6;
    std::cout << std::fixed << std::setprecision(6) << "[marginalizeSlam] time cost = " << sum_time_6 << std::endl;
    // time test

    std::vector<std::shared_ptr<feature>> features_slam_delayed, features_slam_update;

    for (size_t i = 0; i < features_slam.size(); i++)
    {
        if (state_ptr->map_points.find(features_slam.at(i)->feature_id) != state_ptr->map_points.end())
        {
            features_slam_update.push_back(features_slam.at(i));
        }
        else
        {
            features_slam_delayed.push_back(features_slam.at(i));
        }
    }

    std::vector<std::shared_ptr<feature>> features_up_msckf = features_lost;
    features_up_msckf.insert(features_up_msckf.end(), features_marg.begin(), features_marg.end());
    features_up_msckf.insert(features_up_msckf.end(), features_maxtracks.begin(), features_maxtracks.end());

    auto compare_feature = [](const std::shared_ptr<feature> &a, const std::shared_ptr<feature> &b) -> bool {
        size_t size_a = 0;
        size_t size_b = 0;

        for (const auto &pair : a->timestamps)
            size_a += pair.second.size();

        for (const auto &pair : b->timestamps)
            size_b += pair.second.size();

        return size_a < size_b;
    };
    std::sort(features_up_msckf.begin(), features_up_msckf.end(), compare_feature);

    if ((int)features_up_msckf.size() > state_ptr->options.max_msckf_in_update)
        features_up_msckf.erase(features_up_msckf.begin(), features_up_msckf.end() - state_ptr->options.max_msckf_in_update);

    // time test
    boost::posix_time::ptime rT_begin2 = boost::posix_time::microsec_clock::local_time();
    // time test

    updaterMsckf_ptr->update(state_ptr, features_up_msckf);

    // time test
    boost::posix_time::ptime rT_end2 = boost::posix_time::microsec_clock::local_time();
    time_cost = (rT_end2 - rT_begin2).total_microseconds() * 1e-6;
    sum_time_2 = sum_time_2 * n_time_2 + time_cost;
    n_time_2++;
    sum_time_2 = sum_time_2 / n_time_2;
    std::cout << std::fixed << std::setprecision(6) << "[state update without map points] time cost = " << sum_time_2 << std::endl;
    // time test

    propagator_ptr->invalidateCache();

    rT4 = boost::posix_time::microsec_clock::local_time();

    std::vector<std::shared_ptr<feature>> features_slam_update_temp;

    while (!features_slam_update.empty())
    {
        std::vector<std::shared_ptr<feature>> features_up_temp;

        features_up_temp.insert(features_up_temp.begin(), features_slam_update.begin(),
            features_slam_update.begin() + std::min(state_ptr->options.max_slam_in_update, (int)features_slam_update.size()));

        features_slam_update.erase(features_slam_update.begin(),
            features_slam_update.begin() + std::min(state_ptr->options.max_slam_in_update, (int)features_slam_update.size()));

        // time test
        boost::posix_time::ptime rT_begin3 = boost::posix_time::microsec_clock::local_time();
        // time test

        updaterSlam_ptr->update(state_ptr, voxel_map, features_up_temp);

        // time test
        boost::posix_time::ptime rT_end3 = boost::posix_time::microsec_clock::local_time();
        time_cost = (rT_end3 - rT_begin3).total_microseconds() * 1e-6;
        sum_time_3 = sum_time_3 * n_time_3 + time_cost;
        n_time_3++;
        sum_time_3 = sum_time_3 / n_time_3;
        std::cout << std::fixed << std::setprecision(6) << "[state update without map points] time cost = " << sum_time_3 << std::endl;
        // time test

        auto it = state_ptr->map_points.begin();

        while (it != state_ptr->map_points.end())
        {
            mapManagement::changeHostVoxel(voxel_map, (*it).second, odometry_options.voxel_size, odometry_options.max_num_points_in_voxel, odometry_options.min_distance_points, voxels_history);
            it++;
        }

        features_slam_update_temp.insert(features_slam_update_temp.end(), features_up_temp.begin(), features_up_temp.end());
        propagator_ptr->invalidateCache();
    }

    features_slam_update = features_slam_update_temp;
    rT5 = boost::posix_time::microsec_clock::local_time();

    // time test
    boost::posix_time::ptime rT_begin4 = boost::posix_time::microsec_clock::local_time();
    // time test

    updaterSlam_ptr->delayedInit(state_ptr, voxel_map, features_slam_delayed, voxels_history);

    // time test
    boost::posix_time::ptime rT_end4 = boost::posix_time::microsec_clock::local_time();
    time_cost = (rT_end4 - rT_begin4).total_microseconds() * 1e-6;
    sum_time_4 = sum_time_4 * n_time_4 + time_cost;
    n_time_4++;
    sum_time_4 = sum_time_4 / n_time_4;
    std::cout << std::fixed << std::setprecision(6) << "[map point registration] time cost = " << sum_time_4 << std::endl;
    // time test

    rT6 = boost::posix_time::microsec_clock::local_time();

    if (image_measurements.camera_ids.at(0) == 0)
    {
        // time test
        boost::posix_time::ptime rT_begin5 = boost::posix_time::microsec_clock::local_time();
        // time test

        triangulateActiveTracks(image_measurements);

        getRecentVoxel(image_measurements.timestamp, voxels_visit);

        // time test
        boost::posix_time::ptime rT_end5 = boost::posix_time::microsec_clock::local_time();
        time_cost = (rT_end5 - rT_begin5).total_microseconds() * 1e-6;
        sum_time_5 = sum_time_5 * n_time_5 + time_cost;
        n_time_5++;
        sum_time_5 = sum_time_5 / n_time_5;
        std::cout << std::fixed << std::setprecision(6) << "[suitable map point selection] time cost = " << sum_time_5 << std::endl;
        // time test

        good_features_msckf.clear();
    }

    for (auto const &feat : features_up_msckf)
    {
        good_features_msckf.push_back(feat->position_global);
        feat->to_delete = true;
    }

    featureTracker->getFeatureDatabase()->cleanUp();

    if ((int)state_ptr->clones_imu.size() > state_ptr->options.max_clone_size)
    {
        featureTracker->getFeatureDatabase()->cleanUpOldmeasurements(state_ptr->margtimestep());
    }

    if (marginalize_status == MarginalizeStatus::OLD)
    {
        stateHelper::marginalizeOldClone(state_ptr);
    }
    else
    {
        stateHelper::marginalizeNewClone(state_ptr);
    }

    rT7 = boost::posix_time::microsec_clock::local_time();

    double time_track = (rT2 - rT1).total_microseconds() * 1e-6;
    double time_prop = (rT3 - rT2).total_microseconds() * 1e-6;
    double time_msckf = (rT4 - rT3).total_microseconds() * 1e-6;
    double time_slam_update = (rT5 - rT4).total_microseconds() * 1e-6;
    double time_slam_delay = (rT6 - rT5).total_microseconds() * 1e-6;
    double time_marg = (rT7 - rT6).total_microseconds() * 1e-6;
    double time_total = (rT7 - rT1).total_microseconds() * 1e-6;

    // Time test
    /*
    std::cout << std::fixed << "[processImage]: " << time_track << " seconds for tracking" << std::endl;
    std::cout << std::fixed << "[featureUpdate]: " << time_prop << " seconds for propagation" << std::endl;
    std::cout << std::fixed << "[featureUpdate]: " << time_msckf << " seconds for MSCKF update (" << (int)features_up_msckf.size() << " feats)" << std::endl;

    if (state_ptr->options.max_slam_features > 0)
    {
        std::cout << std::fixed << "[featureUpdate]: " << time_slam_update << " seconds for SLAM update (" << (int)state_ptr->map_points.size() << " feats)" << std::endl;
        std::cout << std::fixed << "[featureUpdate]: " << time_slam_delay << " seconds for SLAM delayed init (" << (int)features_slam_delayed.size() << " feats)" << std::endl;
    }

    std::cout << std::fixed << "[featureUpdate]: " << time_marg << " seconds for re-tri & marg (" << (int)state_ptr->clones_imu.size() << " clones in state)" << std::endl;

    std::stringstream ss;
    ss << "[featureUpdate]: " << std::setprecision(4) << time_total << " seconds for total (camera";

    for (const auto &id : image_measurements.camera_ids) ss << " " << id;

    ss << ")" << std::endl;

    std::cout << ss.str();
    */
    // Time test

    if (timelastupdate != -1 && state_ptr->clones_imu.find(timelastupdate) != state_ptr->clones_imu.end())
    {
        Eigen::Matrix<double, 3, 1> dx = state_ptr->imu_ptr->getPos() - state_ptr->clones_imu.at(timelastupdate)->getPos();
        distance += dx.norm();
    }
    timelastupdate = image_measurements.timestamp;

    // display
    pubOdometry(state_ptr, timelastupdate);
    pubPath(state_ptr, timelastupdate);
    pubHistoryPoints(points_history, timelastupdate);
    pubWindowPoints(state_ptr, timelastupdate);
    pubHistoryVoxels(voxels_history, timelastupdate);
    pubVisitVoxels(voxels_visit, timelastupdate);
    // display

    std::cout << std::fixed << std::setprecision(3) << "q_GtoI = " << state_ptr->imu_ptr->getQuat()(0) << " " << state_ptr->imu_ptr->getQuat()(1) << " "
        << state_ptr->imu_ptr->getQuat()(2) << " " << state_ptr->imu_ptr->getQuat()(3) << std::endl;

    std::cout << std::fixed << std::setprecision(3) << "p_IinG = " << state_ptr->imu_ptr->getPos()(0) << " " << state_ptr->imu_ptr->getPos()(1) << " "
        << state_ptr->imu_ptr->getPos()(2) << std::endl;

    std::cout << std::fixed << std::setprecision(2) << "dist = " << distance << " meters" << std::endl;

    std::cout << std::fixed << std::setprecision(4) << "bg = " << state_ptr->imu_ptr->getBg()(0) << " " << state_ptr->imu_ptr->getBg()(1) << " "
        << state_ptr->imu_ptr->getBg()(2) << std::endl;

    std::cout << std::fixed << std::setprecision(4) << "ba = " << state_ptr->imu_ptr->getBa()(0) << " " << state_ptr->imu_ptr->getBa()(1) << " "
        << state_ptr->imu_ptr->getBa()(2) << std::endl;

    std::ofstream foutC(std::string(output_path + "/pose.txt"), std::ios::app);
    foutC.setf(std::ios::scientific, std::ios::floatfield);
    foutC.precision(6);
    foutC << std::fixed << image_measurements.timestamp + state_ptr->calib_dt_imu_cam->value()(0) << " " << state_ptr->imu_ptr->getPos()(0) << " " << state_ptr->imu_ptr->getPos()(1) << " " << state_ptr->imu_ptr->getPos()(2) << " "
        << state_ptr->imu_ptr->getQuat()(0) << " " << state_ptr->imu_ptr->getQuat()(1) << " " << state_ptr->imu_ptr->getQuat()(2) << " " << state_ptr->imu_ptr->getQuat()(3) << std::endl;
    foutC.close();

    if (state_ptr->options.do_calib_camera_timeoffset)
        std::cout << std::fixed << std::setprecision(5) << "camera-imu timeoffset = " << state_ptr->calib_dt_imu_cam->value()(0) << std::endl;

    if (state_ptr->options.do_calib_camera_intrinsics)
    {
        for (int i = 0; i < 2; i++)
        {
            std::shared_ptr<vec> calib = state_ptr->cam_intrinsics.at(i);

            std::cout << std::fixed << std::setprecision(3) << "cam" << (int)i << " intrinsics = " << calib->value()(0) << " " << calib->value()(1) << " "
                << calib->value()(2) << " " << calib->value()(3) << " " << calib->value()(4) << " " << calib->value()(5) << " "
                << calib->value()(6) << " " << calib->value()(7) << std::endl;

        }
    }

    if (state_ptr->options.do_calib_camera_pose)
    {
        for (int i = 0; i < 2; i++)
        {
            std::shared_ptr<poseJpl> calib = state_ptr->calib_cam_imu.at(i);

            std::cout << std::fixed << std::setprecision(3) << "cam" << (int)i << " extrinsics = " << calib->getQuat()(0) << " " << calib->getQuat()(1) << " "
                << calib->getQuat()(2) << " " << calib->getQuat()(3) << " " << calib->getPos()(0) << " " << calib->getPos()(1) << " "
                << calib->getPos()(2) << std::endl;
        }
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.imu_model == ImuModel::KALIBR)
    {
        std::cout << std::fixed << std::setprecision(3) << "q_GYROtoI = " << state_ptr->calib_imu_gyr->value()(0) << " " << state_ptr->calib_imu_gyr->value()(1) << " "
            << state_ptr->calib_imu_gyr->value()(2) << " " << state_ptr->calib_imu_gyr->value()(3) << std::endl;
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.imu_model == ImuModel::RPNG)
    {
        std::cout << std::fixed << std::setprecision(3) << "q_ACCtoI = " << state_ptr->calib_imu_acc->value()(0) << " " << state_ptr->calib_imu_acc->value()(1) << " "
            << state_ptr->calib_imu_acc->value()(2) << " " << state_ptr->calib_imu_acc->value()(3) << std::endl;
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.imu_model == ImuModel::KALIBR)
    {
        std::cout << std::fixed << std::setprecision(4) << "Dw = | " << state_ptr->calib_imu_dw->value()(0) << ", " << state_ptr->calib_imu_dw->value()(1) << ", "
            << state_ptr->calib_imu_dw->value()(2) << " | " << state_ptr->calib_imu_dw->value()(3) << ", " << state_ptr->calib_imu_dw->value()(4)
            << " | " << state_ptr->calib_imu_dw->value()(5) << " |" << std::endl;

        std::cout << std::fixed << std::setprecision(4) << "Da = | " << state_ptr->calib_imu_da->value()(0) << ", " << state_ptr->calib_imu_da->value()(1) << ", "
            << state_ptr->calib_imu_da->value()(2) << " | " << state_ptr->calib_imu_da->value()(3) << ", " << state_ptr->calib_imu_da->value()(4)
            << " | " << state_ptr->calib_imu_da->value()(5) << " |" << std::endl;
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.imu_model == ImuModel::RPNG)
    {
        std::cout << std::fixed << std::setprecision(4) << "Dw = | " << state_ptr->calib_imu_dw->value()(0) << " | " << state_ptr->calib_imu_dw->value()(1) << ", "
            << state_ptr->calib_imu_dw->value()(2) << " | " << state_ptr->calib_imu_dw->value()(3) << ", " << state_ptr->calib_imu_dw->value()(4)
            << ", " << state_ptr->calib_imu_dw->value()(5) << " |" << std::endl;

        std::cout << std::fixed << std::setprecision(4) << "Da = | " << state_ptr->calib_imu_da->value()(0) << " | " << state_ptr->calib_imu_da->value()(1) << ", "
            << state_ptr->calib_imu_da->value()(2) << " | " << state_ptr->calib_imu_da->value()(3) << ", " << state_ptr->calib_imu_da->value()(4)
            << ", " << state_ptr->calib_imu_da->value()(5) << " |" << std::endl;
    }

    if (state_ptr->options.do_calib_imu_intrinsics && state_ptr->options.do_calib_imu_g_sensitivity)
    {
        std::cout << std::fixed << std::setprecision(4) << "Tg = | " << state_ptr->calib_imu_tg->value()(0) << ", " << state_ptr->calib_imu_tg->value()(1) << ", "
            << state_ptr->calib_imu_tg->value()(2) << " | " << state_ptr->calib_imu_tg->value()(3) << ", " << state_ptr->calib_imu_tg->value()(4) << ", "
            << state_ptr->calib_imu_tg->value()(5) << " | " << state_ptr->calib_imu_tg->value()(6) << ", " << state_ptr->calib_imu_tg->value()(7) << ", "
            << state_ptr->calib_imu_tg->value()(8) << " |" << std::endl;
    }

    std::cout << std::endl;
}
    void triangulateActiveTracks(cameraData &image_measurements) {
    boost::posix_time::ptime retri_rT1, retri_rT2, retri_rT3;
    retri_rT1 = boost::posix_time::microsec_clock::local_time();

    assert(state_ptr->clones_imu.find(image_measurements.timestamp) != state_ptr->clones_imu.end());
    active_tracks_time = image_measurements.timestamp;

    // draw
    /*
    active_image = cv::Mat();
    featureTracker->displayActive(active_image, 255, 255, 255, 255, 255, 255, " ");
    if (!active_image.empty())
    {
        active_image = active_image(cv::Rect(0, 0, image_measurements.images.at(0).cols, image_measurements.images.at(0).rows));
    }
    */
    // draw

    active_tracks_pos_world.clear();
    active_tracks_uvd.clear();

    auto last_obs = featureTracker->getLastObs();
    auto last_ids = featureTracker->getLastIds();

    std::map<size_t, Eigen::Matrix3d> active_feat_linsys_A_new;
    std::map<size_t, Eigen::Vector3d> active_feat_linsys_b_new;
    std::map<size_t, int> active_feat_linsys_count_new;
    std::unordered_map<size_t, Eigen::Vector3d> active_tracks_pos_world_new;

    std::map<size_t, cv::Point2f> feat_uvs_in_cam_0;

    // display
    assert(state_ptr->clones_frame.find(active_tracks_time) != state_ptr->clones_frame.end());

    cv::Mat img_left = image_measurements.images.at(0);
    cv::Mat img_right = image_measurements.images.at(1);

    cv::Mat color_left = convertCvImage(img_left);
    cv::Mat color_right = convertCvImage(img_right);

    cv::Vec3b color_feat = cv::Vec3b(0, 255, 0);
    // display

    for (auto const &cam_id : image_measurements.camera_ids)
    {
        Eigen::Matrix3d R_GtoI = state_ptr->clones_imu.at(active_tracks_time)->getRot();
        Eigen::Vector3d p_IinG = state_ptr->clones_imu.at(active_tracks_time)->getPos();

        Eigen::Matrix3d R_ItoC = state_ptr->calib_cam_imu.at(cam_id)->getRot();
        Eigen::Vector3d p_IinC = state_ptr->calib_cam_imu.at(cam_id)->getPos();

        Eigen::Matrix3d R_GtoCi = R_ItoC * R_GtoI;
        Eigen::Vector3d p_CiinG = p_IinG - R_GtoCi.transpose() * p_IinC;

        assert(last_obs.find(cam_id) != last_obs.end());
        assert(last_ids.find(cam_id) != last_ids.end());

        for (size_t i = 0; i < last_obs.at(cam_id).size(); i++)
        {
            size_t feature_id = last_ids.at(cam_id).at(i);
            cv::Point2f pt_d = last_obs.at(cam_id).at(i).pt;

            // display
            if (pt_d.x > 5 && pt_d.x < wG[0] - 5 && pt_d.y > 5 && pt_d.y < hG[0] - 5)
            {
                if (cam_id == 0)
                    drawPointL(color_left, pt_d.x, pt_d.y, color_feat);
                else
                    drawPointL(color_right, pt_d.x, pt_d.y, color_feat);
            }
            // display

            if (cam_id == 0) feat_uvs_in_cam_0[feature_id] = pt_d;

            if (state_ptr->map_points.find(feature_id) != state_ptr->map_points.end()) continue;

            cv::Point2f pt_n = state_ptr->cam_intrinsics_cameras.at(cam_id)->undistortCV(pt_d);
            Eigen::Matrix<double, 3, 1> b_i;
            b_i << pt_n.x, pt_n.y, 1;
            b_i = R_GtoCi.transpose() * b_i;
            b_i = b_i / b_i.norm();
            Eigen::Matrix3d B_perp = quatType::skewSymmetric(b_i);

            Eigen::Matrix3d Ai = B_perp.transpose() * B_perp;
            Eigen::Vector3d bi = Ai * p_CiinG;

            if (active_feat_linsys_A.find(feature_id) == active_feat_linsys_A.end())
            {
                active_feat_linsys_A_new.insert({feature_id, Ai});
                active_feat_linsys_b_new.insert({feature_id, bi});
                active_feat_linsys_count_new.insert({feature_id, 1});
            }
            else
            {
                active_feat_linsys_A_new[feature_id] = Ai + active_feat_linsys_A[feature_id];
                active_feat_linsys_b_new[feature_id] = bi + active_feat_linsys_b[feature_id];
                active_feat_linsys_count_new[feature_id] = 1 + active_feat_linsys_count[feature_id];
            }

            if (active_feat_linsys_count_new.at(feature_id) > 3)
            {
                Eigen::Matrix3d A = active_feat_linsys_A_new[feature_id];
                Eigen::Vector3d b = active_feat_linsys_b_new[feature_id];
                Eigen::MatrixXd p_FinG = A.colPivHouseholderQr().solve(b);
                Eigen::MatrixXd p_FinCi = R_GtoCi * (p_FinG - p_CiinG);

                Eigen::JacobiSVD<Eigen::Matrix3d> svd(A);
                Eigen::MatrixXd singularValues;
                singularValues.resize(svd.singularValues().rows(), 1);
                singularValues = svd.singularValues();
                double cond_A = singularValues(0, 0) / singularValues(singularValues.rows() - 1, 0);

                if (std::abs(cond_A) <= odometry_options.featinit_options.max_cond_number && p_FinCi(2, 0) >= odometry_options.featinit_options.min_dist &&
                    p_FinCi(2, 0) <= odometry_options.featinit_options.max_dist && !std::isnan(p_FinCi.norm()))
                {
                    active_tracks_pos_world_new[feature_id] = p_FinG;
                }
            }
        }
    }

    // display
    cv::Mat stereo_image;
    cv::vconcat(color_left, color_right, stereo_image);

    /*
    cv::imshow("Stereo Image", stereo_image);
    cv::waitKey(1);
    */
    pubFeatImage(stereo_image, active_tracks_time);

    img_left.release();
    img_right.release();

    color_left.release();
    color_right.release();

    stereo_image.release();
    // display

    size_t total_triangulated = active_tracks_pos_world.size();

    active_feat_linsys_A = active_feat_linsys_A_new;
    active_feat_linsys_b = active_feat_linsys_b_new;
    active_feat_linsys_count = active_feat_linsys_count_new;
    active_tracks_pos_world = active_tracks_pos_world_new;
    retri_rT2 = boost::posix_time::microsec_clock::local_time();

    if (active_tracks_pos_world.empty() && state_ptr->map_points.empty()) return;

    for (const auto &feat : state_ptr->map_points)
    {
        Eigen::Vector3d p_FinG = feat.second->getPointXYZ(false);
        active_tracks_pos_world[feat.second->feature_id] = p_FinG;
    }

    std::shared_ptr<vec> distortion = state_ptr->cam_intrinsics.at(0);
    std::shared_ptr<poseJpl> calibration = state_ptr->calib_cam_imu.at(0);
    Eigen::Matrix<double, 3, 3> R_ItoC = calibration->getRot();
    Eigen::Matrix<double, 3, 1> p_IinC = calibration->getPos();

    std::shared_ptr<poseJpl> clone_Ii = state_ptr->clones_imu.at(active_tracks_time);
    Eigen::Matrix3d R_GtoIi = clone_Ii->getRot();
    Eigen::Vector3d p_IiinG = clone_Ii->getPos();

    for (const auto &feat : active_tracks_pos_world)
    {
        if (feat_uvs_in_cam_0.find(feat.first) == feat_uvs_in_cam_0.end()) continue;

        Eigen::Vector3d p_FinIi = R_GtoIi * (feat.second - p_IiinG);
        Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;

        double depth = p_FinCi(2);
        Eigen::Vector2d uv_dist;

        if (feat_uvs_in_cam_0.find(feat.first) != feat_uvs_in_cam_0.end())
        {
            uv_dist << (double)feat_uvs_in_cam_0.at(feat.first).x, (double)feat_uvs_in_cam_0.at(feat.first).y;
        }
        else
        {
            Eigen::Vector2d uv_norm;
            uv_norm << p_FinCi(0) / depth, p_FinCi(1) / depth;
            uv_dist = state_ptr->cam_intrinsics_cameras.at(0)->distortD(uv_norm);
        }

        if (depth < 0.1) continue;

        int width = state_ptr->cam_intrinsics_cameras.at(0)->w();
        int height = state_ptr->cam_intrinsics_cameras.at(0)->h();

        if (uv_dist(0) < 0 || (int)uv_dist(0) >= width || uv_dist(1) < 0 || (int)uv_dist(1) >= height)
        {
            continue;
        }

        Eigen::Vector3d uvd;
        uvd << uv_dist, depth;
        active_tracks_uvd.insert({feat.first, uvd});
    }
    retri_rT3 = boost::posix_time::microsec_clock::local_time();

    // Time test
    /*
    std::cout << std::fixed << "[triangulateActiveTracks]: " << (retri_rT2 - retri_rT1).total_microseconds() * 1e-6 << " seconds for triangulation ("
        << total_triangulated << " tri of " << active_feat_linsys_A.size() << " active)" << std::endl;

    std::cout << std::fixed << "[triangulateActiveTracks]: " << (retri_rT3 - retri_rT2).total_microseconds() * 1e-6 << " seconds for re-projection into current" << std::endl;

    std::cout << std::fixed << "[triangulateActiveTracks]: " << (retri_rT3 - retri_rT1).total_microseconds() * 1e-6 << " seconds total" << std::endl;
    */
    // Time test
}
    void getRecentVoxel(double timestamp, pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_visit) {
    std::vector<Eigen::Matrix<short, 3, 1>>().swap(recent_voxels);

    // display
    voxels_visit->clear();
    // display

    for (const auto &point : active_tracks_pos_world)
    {
        short kx = static_cast<short>(point.second[0] / odometry_options.voxel_size);
        short ky = static_cast<short>(point.second[1] / odometry_options.voxel_size);
        short kz = static_cast<short>(point.second[2] / odometry_options.voxel_size);

        for (short kxx = kx - odometry_options.nb_voxels_visited; kxx < kx + odometry_options.nb_voxels_visited + 1; kxx++)
        {
            for (short kyy = ky - odometry_options.nb_voxels_visited; kyy < ky + odometry_options.nb_voxels_visited + 1; kyy++)
            {
                for (short kzz = kz - odometry_options.nb_voxels_visited; kzz < kz + odometry_options.nb_voxels_visited + 1; kzz++)
                {
                    voxelHashMap::iterator search = voxel_map.find(voxel(kxx, kyy, kzz));

                    if(search != voxel_map.end())
                    {
                        auto &voxel_block = (search.value());

                        if (voxel_block.last_visit_time < timestamp && voxel_block.NumPoints() > 0)
                        {
                            Eigen::Matrix<short, 3, 1> voxel_temp;

                            voxel_temp[0] = kxx;
                            voxel_temp[1] = kyy;
                            voxel_temp[2] = kzz;

                            recent_voxels.push_back(voxel_temp);
                            voxel_block.last_visit_time = timestamp;

                            // display
                            pcl::PointXYZI point_temp;

                            point_temp.x = kxx >= 0 ? (kxx + 0.5) * odometry_options.voxel_size : (kxx - 0.5) * odometry_options.voxel_size;
                            point_temp.y = kyy >= 0 ? (kyy + 0.5) * odometry_options.voxel_size : (kyy - 0.5) * odometry_options.voxel_size;
                            point_temp.z = kzz >= 0 ? (kzz + 0.5) * odometry_options.voxel_size : (kzz - 0.5) * odometry_options.voxel_size;
                            point_temp.intensity = 1;

                            voxels_visit->points.push_back(point_temp);
                            // display
                        }
                    }
                }
            }
        }
    }
}
    bool tryToInitialize(cameraData &image_measurements) {
    camera_queue_init.push_back(image_measurements.timestamp);

    assert(newest_fh->timestamp == image_measurements.timestamp);
    frame_queue_init.push_back(newest_fh);

    double timestamp;
    Eigen::MatrixXd covariance;
    std::vector<std::shared_ptr<baseType>> order;
    auto init_rT1 = boost::posix_time::microsec_clock::local_time();

    bool success = initializer_ptr->initialize(timestamp, covariance, order, state_ptr->imu_ptr, true);

    if (success)
    {
        stateHelper::setInitialCovariance(state_ptr, covariance, order);

        state_ptr->timestamp = timestamp;
        startup_time = timestamp;

        featureTracker->getFeatureDatabase()->cleanUpOldmeasurements(state_ptr->timestamp);
        featureTracker->setNumFeatures(std::floor((double)odometry_options.num_pts / (double)2.0));

        auto init_rT2 = boost::posix_time::microsec_clock::local_time();
        std::cout << std::fixed << "[initialize]: Successful initialization in " << (init_rT2 - init_rT1).total_microseconds() * 1e-6 << " seconds" << std::endl;
        std::cout << std::fixed << "[initialize]: position = " << state_ptr->imu_ptr->getPos()(0) << ", " << state_ptr->imu_ptr->getPos()(1) << ", "
            << state_ptr->imu_ptr->getPos()(2) << std::endl;
        std::cout << std::fixed << "[initialize]: orientation = " << state_ptr->imu_ptr->getQuat()(0) << ", " << state_ptr->imu_ptr->getQuat()(1) << ", " << state_ptr->imu_ptr->getQuat()(2)
            << ", " << state_ptr->imu_ptr->getQuat()(3) << std::endl;
        std::cout << std::fixed << "[initialize]: velocity = " << state_ptr->imu_ptr->getVel()(0) << ", " << state_ptr->imu_ptr->getVel()(1) << ", "
            << state_ptr->imu_ptr->getVel()(2) << std::endl;
        std::cout << std::fixed << "[initialize]: bias accel = " << state_ptr->imu_ptr->getBa()(0) << ", " << state_ptr->imu_ptr->getBa()(1) << ", "
            << state_ptr->imu_ptr->getBa()(2) << std::endl;
        std::cout << std::fixed << "[initialize]: bias gyro = " << state_ptr->imu_ptr->getBg()(0) << ", " << state_ptr->imu_ptr->getBg()(1) << ", "
            << state_ptr->imu_ptr->getBg()(2) << std::endl;

        std::vector<double> camera_timestamps_to_init;
        for (size_t i = 0; i < camera_queue_init.size(); i++)
        {
            if (camera_queue_init.at(i) > timestamp)
                camera_timestamps_to_init.push_back(camera_queue_init.at(i));
        }

        std::vector<std::shared_ptr<frame>> camera_frames_to_init;
        for (size_t i = 0; i < frame_queue_init.size(); i++)
        {
            if (frame_queue_init.at(i)->timestamp > timestamp)
                camera_frames_to_init.push_back(frame_queue_init.at(i));
        }

        size_t clone_rate = (size_t)((double)camera_timestamps_to_init.size() / (double)odometry_options.state_options.max_clone_size) + 1;

        for (size_t i = 0; i < camera_timestamps_to_init.size(); i += clone_rate)
        {
            propagator_ptr->propagateAndClone(state_ptr, camera_timestamps_to_init.at(i));

            if (state_ptr->clones_frame.find(camera_frames_to_init.at(i)->timestamp) == state_ptr->clones_frame.end())
                state_ptr->clones_frame[camera_frames_to_init.at(i)->timestamp] = camera_frames_to_init.at(i);

            stateHelper::marginalizeOldClone(state_ptr);
        }

        std::cout << "[initialize]: Moved the state forward " << state_ptr->timestamp - timestamp << " seconds" << std::endl;
        camera_queue_init.clear();

        frame_queue_init.clear();

        return true;
    }
    else
    {
        auto init_rT2 = boost::posix_time::microsec_clock::local_time();
        std::cout << std::fixed << "[initialize]: Failed initialization in " << (init_rT2 - init_rT1).total_microseconds() * 1e-6 << " seconds" << std::endl;

        double oldest_time = image_measurements.timestamp - odometry_options.init_options.init_window_time - 0.10;

        auto it0 = camera_queue_init.begin();

        while (it0 != camera_queue_init.end())
        {
            if (*it0 < oldest_time)
                it0 = camera_queue_init.erase(it0);
            else
                it0++;
        }

        auto it1 = frame_queue_init.begin();

        while (it1 != frame_queue_init.end())
        {
            if ((*it1)->timestamp < oldest_time)
            {
                assert((*it1)->v_feat_ptr[0].size() == 0);
                assert((*it1)->v_feat_ptr[1].size() == 0);
                (*it1)->release();
                it1 = frame_queue_init.erase(it1);
            }
            else
                it1++;
        }

        return false;
    }
}
    std::queue<cameraData> camera_buffer;
    std::queue<imuData> imu_buffer;
    std::map<int, double> camera_last_timestamp;
    odometryOptions odometry_options;
    std::shared_ptr<trackKLT> featureTracker;
    std::shared_ptr<inertialInitializer> initializer_ptr;
    double startup_time;
    double current_time;
    double time_newest_imu;
    std::vector<std::pair<double, std::pair<Eigen::Vector3d, Eigen::Vector3d>>> imu_meas;
    std::vector<imuState> imu_states;
    std::shared_ptr<state> state_ptr;
    imuData last_imu_data;
    std::shared_ptr<propagator> propagator_ptr;
    std::shared_ptr<updaterMsckf> updaterMsckf_ptr;
    std::shared_ptr<updaterSlam> updaterSlam_ptr;
    std::shared_ptr<gammaPixel> gammaPixel_ptr;
    std::shared_ptr<frame> newest_fh;
    std::vector<double> camera_queue_init;
    std::vector<std::shared_ptr<frame>> frame_queue_init;
    voxelHashMap voxel_map;
    int frame_count;
    MarginalizeStatus marginalize_status;
    double last_time_image;
    std::vector<Eigen::Vector3d> good_features_msckf;
    double active_tracks_time = -1;
    std::unordered_map<size_t, Eigen::Vector3d> active_tracks_pos_world;
    std::unordered_map<size_t, Eigen::Vector3d> active_tracks_uvd;
    cv::Mat active_image;
    std::map<size_t, Eigen::Matrix3d> active_feat_linsys_A;
    std::map<size_t, Eigen::Vector3d> active_feat_linsys_b;
    std::map<size_t, int> active_feat_linsys_count;
    std::vector<Eigen::Matrix<short, 3, 1>> recent_voxels;
    std::ofstream of_statistics;
    boost::posix_time::ptime rT1, rT2, rT3, rT4, rT5, rT6, rT7;
    double timelastupdate = -1;
    double distance = 0;
    double sum_time_1 = 0.0;
    double sum_time_2 = 0.0;
    double sum_time_3 = 0.0;
    double sum_time_4 = 0.0;
    double sum_time_5 = 0.0;
    double sum_time_6 = 0.0;
    double sum_time_7 = 0.0;
    double sum_time_sum = 0.0;
    int n_time_1 = 0;
    int n_time_2 = 0;
    int n_time_3 = 0;
    int n_time_4 = 0;
    int n_time_5 = 0;
    int n_time_6 = 0;
    int n_time_7 = 0;
    int n_time_sum = 0;
  public:
    voxelStereoVio() {
    readParameters();

    initialValue();

    allocateMemory();

    sub_imu = nh.subscribe<sensor_msgs::Imu>(imu_topic, 500, &voxelStereoVio::imuHandler, this);

    auto sub_img_left = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, image_left_topic, 5);
    auto sub_img_right = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, image_right_topic, 5);
    auto sync = std::make_shared<message_filters::Synchronizer<SyncStereoImage>>(SyncStereoImage(10), *sub_img_left, *sub_img_right);
    sync->registerCallback(boost::bind(&voxelStereoVio::stereoImageHandler, this, _1, _2, 0, 1));

    sync_cam.push_back(sync);
    sync_subs_cam.push_back(sub_img_left);
    sync_subs_cam.push_back(sub_img_right);

    // display
    pub_feat_image = it.advertise("camera/stereo_feat_image", 5);
    pub_odom = nh.advertise<nav_msgs::Odometry>("vio/odom", 5);
    pub_path = nh.advertise<nav_msgs::Path>("vio/path", 5);
    pub_points_history = nh.advertise<sensor_msgs::PointCloud2>("vio/history_map_points", 2);
    pub_points_window = nh.advertise<sensor_msgs::PointCloud2>("vio/window_map_points", 2);
    pub_voxels_history = nh.advertise<sensor_msgs::PointCloud2>("vio/history_voxels", 2);
    pub_voxels_visit = nh.advertise<sensor_msgs::PointCloud2>("vio/visit_voxels", 2);

    points_history.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
    points_window.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
    voxels_history.reset(new pcl::PointCloud<pcl::PointXYZI>());
    voxels_visit.reset(new pcl::PointCloud<pcl::PointXYZI>());
    // display

    // odometry_options.recordParameters();
}
    void readParameters() {
    int para_int;
    double para_double;
    bool para_bool;
    std::string str_temp;

    nh.param<std::string>("common/image_left_topic", image_left_topic, "/cam0/image_raw");
    nh.param<std::string>("common/image_right_topic", image_right_topic, "/cam1/image_raw");
    nh.param<std::string>("common/imu_topic", imu_topic, "/imu0");
    nh.param<std::string>("output_path", output_path, "");

    nh.param<bool>("state_parameter/use_fej", para_bool, false); odometry_options.state_options.do_fej = para_bool;
    nh.param<bool>("state_parameter/calib_cam_extrinsics", para_bool, false); odometry_options.state_options.do_calib_camera_pose = para_bool;
    nh.param<bool>("state_parameter/calib_cam_intrinsics", para_bool, false); odometry_options.state_options.do_calib_camera_intrinsics = para_bool;
    nh.param<bool>("state_parameter/calib_cam_timeoffset", para_bool, false); odometry_options.state_options.do_calib_camera_timeoffset = para_bool;
    nh.param<bool>("state_parameter/calib_imu_intrinsics", para_bool, false); odometry_options.state_options.do_calib_imu_intrinsics = para_bool;
    nh.param<bool>("state_parameter/calib_imu_g_sensitivity", para_bool, false); odometry_options.state_options.do_calib_imu_g_sensitivity = para_bool;

    nh.param<std::string>("state_parameter/imu_intrinsics_model", str_temp, "kalibr");
    if (str_temp == "kalibr" || str_temp == "calibrated") odometry_options.state_options.imu_model = ImuModel::KALIBR;
    else if (str_temp == "rpng") odometry_options.state_options.imu_model = ImuModel::RPNG;
    else {
        std::cout << "Invalid IMU model: " << str_temp << std::endl;
        std::cout << "Please select a valid model: kalibr, rpng." << std::endl;
        std::exit(EXIT_FAILURE);
    }
    if (str_temp == "calibrated")
    {
        odometry_options.state_options.do_calib_imu_intrinsics = false;
        odometry_options.state_options.do_calib_imu_g_sensitivity = false;
    }

    nh.param<int>("state_parameter/max_clones", para_int, 11); odometry_options.state_options.max_clone_size = para_int;
    nh.param<int>("state_parameter/max_slam", para_int, 25); odometry_options.state_options.max_slam_features = para_int;
    nh.param<int>("state_parameter/max_slam_in_update", para_int, 1000); odometry_options.state_options.max_slam_in_update = para_int;
    nh.param<int>("state_parameter/max_msckf_in_update", para_int, 1000); odometry_options.state_options.max_msckf_in_update = para_int;

    nh.param<double>("initializer_parameter/init_window_time", para_double, 1.0); odometry_options.init_options.init_window_time = para_double;
    nh.param<double>("initializer_parameter/init_imu_thresh", para_double, 1.0); odometry_options.init_options.init_imu_thresh = para_double;
    nh.param<double>("initializer_parameter/init_max_disparity", para_double, 1.0); odometry_options.init_options.init_max_disparity = para_double;
    nh.param<int>("initializer_parameter/init_max_features", para_int, 50); odometry_options.init_options.init_max_features = para_int;
    nh.param<bool>("initializer_parameter/init_dyn_use", para_bool, false); odometry_options.init_options.init_dyn_use = para_bool;
    nh.param<bool>("initializer_parameter/init_dyn_mle_opt_calib", para_bool, false); odometry_options.init_options.init_dyn_mle_opt_calib = para_bool;
    nh.param<int>("initializer_parameter/init_dyn_mle_max_iter", para_int, 20); odometry_options.init_options.init_dyn_mle_max_iter = para_int;
    nh.param<int>("initializer_parameter/init_dyn_mle_max_threads", para_int, 20); odometry_options.init_options.init_dyn_mle_max_threads = para_int;
    nh.param<double>("initializer_parameter/init_dyn_mle_max_time", para_double, 5.0); odometry_options.init_options.init_dyn_mle_max_time = para_double;
    nh.param<int>("initializer_parameter/init_dyn_num_pose", para_int, 5); odometry_options.init_options.init_dyn_num_pose = para_int;
    nh.param<double>("initializer_parameter/init_dyn_min_deg", para_double, 45.0); odometry_options.init_options.init_dyn_min_deg = para_double;
    nh.param<double>("initializer_parameter/init_dyn_inflation_ori", para_double, 10.0); odometry_options.init_options.init_dyn_inflation_orientation = para_double;
    nh.param<double>("initializer_parameter/init_dyn_inflation_vel", para_double, 10.0); odometry_options.init_options.init_dyn_inflation_velocity = para_double;
    nh.param<double>("initializer_parameter/init_dyn_inflation_bg", para_double, 100.0); odometry_options.init_options.init_dyn_inflation_bias_gyro = para_double;
    nh.param<double>("initializer_parameter/init_dyn_inflation_ba", para_double, 100.0); odometry_options.init_options.init_dyn_inflation_bias_accel = para_double;
    nh.param<double>("initializer_parameter/init_dyn_min_rec_cond", para_double, 1e-15); odometry_options.init_options.init_dyn_min_rec_cond = para_double;

    std::vector<double> v_bias_acc, v_bias_gyr;
    nh.param<std::vector<double>>("initializer_parameter/init_dyn_bias_g", v_bias_gyr, std::vector<double>());
    nh.param<std::vector<double>>("initializer_parameter/init_dyn_bias_a", v_bias_acc, std::vector<double>());
    odometry_options.init_options.init_dyn_bias_g << v_bias_gyr.at(0), v_bias_gyr.at(1), v_bias_gyr.at(2);
    odometry_options.init_options.init_dyn_bias_a << v_bias_acc.at(0), v_bias_acc.at(1), v_bias_acc.at(2);

    nh.param<double>("initializer_parameter/gravity_mag", para_double, 9.81); odometry_options.init_options.gravity_mag = para_double;
    nh.param<bool>("initializer_parameter/downsample_cameras", para_bool, false); odometry_options.init_options.downsample_cameras = para_bool;

    double calib_camimu_dt_left, calib_camimu_dt_right;
    nh.param<double>("camera_parameter/timeshift_cam_imu_left", calib_camimu_dt_left, 0.0);
    nh.param<double>("camera_parameter/timeshift_cam_imu_right", calib_camimu_dt_right, 0.0);
    odometry_options.calib_camimu_dt = calib_camimu_dt_left;

    std::string dist_model_left, dist_model_right;
    nh.param<std::string>("camera_parameter/distortion_model_left", dist_model_left, "radtan");
    nh.param<std::string>("camera_parameter/distortion_model_right", dist_model_right, "radtan");

    std::vector<double> cam_calib_1_left = {1, 1, 0, 0};
    std::vector<double> cam_calib_1_right = {1, 1, 0, 0};
    std::vector<double> cam_calib_2_left = {0, 0, 0, 0};
    std::vector<double> cam_calib_2_right = {0, 0, 0, 0};
    nh.param<std::vector<double>>("camera_parameter/intrinsics_left", cam_calib_1_left, std::vector<double>());
    nh.param<std::vector<double>>("camera_parameter/intrinsics_right", cam_calib_1_right, std::vector<double>());
    nh.param<std::vector<double>>("camera_parameter/distortion_coeffs_left", cam_calib_2_left, std::vector<double>());
    nh.param<std::vector<double>>("camera_parameter/distortion_coeffs_right", cam_calib_2_right, std::vector<double>());
    Eigen::VectorXd cam_calib_left = Eigen::VectorXd::Zero(8);
    Eigen::VectorXd cam_calib_right = Eigen::VectorXd::Zero(8);
    cam_calib_left << cam_calib_1_left.at(0), cam_calib_1_left.at(1), cam_calib_1_left.at(2), cam_calib_1_left.at(3),
                      cam_calib_2_left.at(0), cam_calib_2_left.at(1), cam_calib_2_left.at(2), cam_calib_2_left.at(3);
    cam_calib_right << cam_calib_1_right.at(0), cam_calib_1_right.at(1), cam_calib_1_right.at(2), cam_calib_1_right.at(3),
                       cam_calib_2_right.at(0), cam_calib_2_right.at(1), cam_calib_2_right.at(2), cam_calib_2_right.at(3);

    cam_calib_left(0) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_left(1) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_left(2) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_left(3) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;

    cam_calib_right(0) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_right(1) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_right(2) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    cam_calib_right(3) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;

    std::vector<int> matrix_wh_left = {1, 1};
    nh.param<std::vector<int>>("camera_parameter/resolution_left", matrix_wh_left, std::vector<int>());
    matrix_wh_left.at(0) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    matrix_wh_left.at(1) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;

    std::vector<int> matrix_wh_right = {1, 1};
    nh.param<std::vector<int>>("camera_parameter/resolution_right", matrix_wh_right, std::vector<int>());
    matrix_wh_right.at(0) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;
    matrix_wh_right.at(1) /= (odometry_options.init_options.downsample_cameras) ? 2.0 : 1.0;

    assert(matrix_wh_left.at(0) == matrix_wh_right.at(0));
    assert(matrix_wh_left.at(1) == matrix_wh_right.at(1));

    wG[0] = matrix_wh_left.at(0);
    hG[0] = matrix_wh_left.at(1);

    std::vector<double> v_T_imu_cam_left;
    nh.param<std::vector<double>>("camera_parameter/T_imu_cam_left", v_T_imu_cam_left, std::vector<double>());
    Eigen::Matrix4d T_imu_cam_left = mat44FromArray(v_T_imu_cam_left);

    std::vector<double> v_T_imu_cam_right;
    nh.param<std::vector<double>>("camera_parameter/T_imu_cam_right", v_T_imu_cam_right, std::vector<double>());
    Eigen::Matrix4d T_imu_cam_right = mat44FromArray(v_T_imu_cam_right);

    Eigen::Matrix<double, 7, 1> cam_eigen_left;
    cam_eigen_left.block<4, 1>(0, 0) = quatType::rotToQuat(T_imu_cam_left.block<3, 3>(0, 0).transpose());
    cam_eigen_left.block<3, 1>(4, 0) = - T_imu_cam_left.block<3, 3>(0, 0).transpose() * T_imu_cam_left.block<3, 1>(0, 3);

    Eigen::Matrix<double, 7, 1> cam_eigen_right;
    cam_eigen_right.block<4, 1>(0, 0) = quatType::rotToQuat(T_imu_cam_right.block<3, 3>(0, 0).transpose());
    cam_eigen_right.block<3, 1>(4, 0) = - T_imu_cam_right.block<3, 3>(0, 0).transpose() * T_imu_cam_right.block<3, 1>(0, 3);

    if (dist_model_left == "equidistant")
    {
        odometry_options.init_options.camera_intrinsics.insert({0, std::make_shared<cameraEqui>(matrix_wh_left.at(0), matrix_wh_left.at(1))});
        odometry_options.init_options.camera_intrinsics.at(0)->setValue(cam_calib_left);

        odometry_options.camera_intrinsics.insert({0, std::make_shared<cameraEqui>(matrix_wh_left.at(0), matrix_wh_left.at(1))});
        odometry_options.camera_intrinsics.at(0)->setValue(cam_calib_left);
    }
    else
    {
        odometry_options.init_options.camera_intrinsics.insert({0, std::make_shared<cameraRadtan>(matrix_wh_left.at(0), matrix_wh_left.at(1))});
        odometry_options.init_options.camera_intrinsics.at(0)->setValue(cam_calib_left);

        odometry_options.camera_intrinsics.insert({0, std::make_shared<cameraRadtan>(matrix_wh_left.at(0), matrix_wh_left.at(1))});
        odometry_options.camera_intrinsics.at(0)->setValue(cam_calib_left);
    }
    odometry_options.init_options.camera_extrinsics.insert({0, cam_eigen_left});
    odometry_options.camera_extrinsics.insert({0, cam_eigen_left});

    if (dist_model_right == "equidistant")
    {
        odometry_options.init_options.camera_intrinsics.insert({1, std::make_shared<cameraEqui>(matrix_wh_right.at(0), matrix_wh_right.at(1))});
        odometry_options.init_options.camera_intrinsics.at(1)->setValue(cam_calib_right);

        odometry_options.camera_intrinsics.insert({1, std::make_shared<cameraEqui>(matrix_wh_right.at(0), matrix_wh_right.at(1))});
        odometry_options.camera_intrinsics.at(1)->setValue(cam_calib_right);
    }
    else
    {
        odometry_options.init_options.camera_intrinsics.insert({1, std::make_shared<cameraRadtan>(matrix_wh_right.at(0), matrix_wh_right.at(1))});
        odometry_options.init_options.camera_intrinsics.at(1)->setValue(cam_calib_right);

        odometry_options.camera_intrinsics.insert({1, std::make_shared<cameraRadtan>(matrix_wh_right.at(0), matrix_wh_right.at(1))});
        odometry_options.camera_intrinsics.at(1)->setValue(cam_calib_right);
    }
    odometry_options.init_options.camera_extrinsics.insert({1, cam_eigen_right});
    odometry_options.camera_extrinsics.insert({1, cam_eigen_right});

    nh.param<bool>("odometry_parameter/use_mask", para_bool, false); odometry_options.use_mask = para_bool;
    if (odometry_options.use_mask)
    {
        std::string mask_left_path, mask_right_path;
        nh.param<std::string>("camera_parameter/mask_left_path", str_temp, ""); mask_left_path = str_temp;
        nh.param<std::string>("camera_parameter/mask_right_path", str_temp, ""); mask_right_path = str_temp;

        if (!boost::filesystem::exists(mask_left_path))
        {
            std::cout << "Invalid mask path: " << mask_left_path << std::endl;
            std::exit(EXIT_FAILURE);
        }
        cv::Mat mask_left = cv::imread(mask_left_path, cv::IMREAD_GRAYSCALE);

        if (!boost::filesystem::exists(mask_right_path))
        {
            std::cout << "Invalid mask path: " << mask_right_path << std::endl;
            std::exit(EXIT_FAILURE);
        }
        cv::Mat mask_right = cv::imread(mask_right_path, cv::IMREAD_GRAYSCALE);

        if (mask_left.cols != odometry_options.camera_intrinsics.at(0)->w() || mask_left.rows != odometry_options.camera_intrinsics.at(0)->h())
        {
            std::cout << "Mask size does not match left camera!" << std::endl;
            std::exit(EXIT_FAILURE);
        }

        if (mask_right.cols != odometry_options.camera_intrinsics.at(1)->w() || mask_right.rows != odometry_options.camera_intrinsics.at(1)->h())
        {
            std::cout << "Mask size does not match right camera!" << std::endl;
            std::exit(EXIT_FAILURE);
        }

        odometry_options.masks.insert({0, mask_left});
        odometry_options.masks.insert({1, mask_right});
    }

    nh.param<double>("imu_parameter/gyroscope_noise_density", para_double, 1.6968e-04); odometry_options.init_options.sigma_w = para_double;
    nh.param<double>("imu_parameter/gyroscope_random_walk", para_double, 1.9393e-05); odometry_options.init_options.sigma_wb = para_double;
    nh.param<double>("imu_parameter/accelerometer_noise_density", para_double, 2.0000e-3); odometry_options.init_options.sigma_a = para_double;
    nh.param<double>("imu_parameter/accelerometer_random_walk", para_double, 3.0000e-03); odometry_options.init_options.sigma_ab = para_double;
    nh.param<double>("imu_parameter/sigma_pix", para_double, 1.0); odometry_options.init_options.sigma_pix = para_double;

    std::vector<double> v_Tw;
    nh.param<std::vector<double>>("imu_parameter/Tw", v_Tw, std::vector<double>());
    Eigen::Matrix3d Tw = mat33FromArray(v_Tw);
    std::vector<double> v_Ta;
    nh.param<std::vector<double>>("imu_parameter/Ta", v_Ta, std::vector<double>());
    Eigen::Matrix3d Ta = mat33FromArray(v_Ta);
    std::vector<double> v_R_acc_imu;
    nh.param<std::vector<double>>("imu_parameter/R_acc_imu", v_R_acc_imu, std::vector<double>());
    Eigen::Matrix3d R_acc_imu = mat33FromArray(v_R_acc_imu);
    std::vector<double> v_R_gyr_imu;
    nh.param<std::vector<double>>("imu_parameter/R_gyr_imu", v_R_gyr_imu, std::vector<double>());
    Eigen::Matrix3d R_gyr_imu = mat33FromArray(v_R_gyr_imu);
    std::vector<double> v_Tg;
    nh.param<std::vector<double>>("imu_parameter/Tg", v_Tg, std::vector<double>());
    Eigen::Matrix3d Tg = mat33FromArray(v_Tg);

    Eigen::Matrix3d Dw = Tw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity());
    Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity());
    Eigen::Matrix3d R_imu_acc = R_acc_imu.transpose();
    Eigen::Matrix3d R_imu_gyr = R_gyr_imu.transpose();

    if (std::isnan(Tw.norm()) || std::isnan(Dw.norm()))
    {
        std::cout << "gyroscope has bad intrinsic values!" << std::endl;
        std::cout << "Tw = " << Tw << std::endl;
        std::cout << "Dw = " << Dw << std::endl;
        std::exit(EXIT_FAILURE);
    }

    if (std::isnan(Ta.norm()) || std::isnan(Da.norm()))
    {
        std::cout << "accelerometer has bad intrinsic values!" << std::endl;
        std::cout << "Ta = " << Ta << std::endl;
        std::cout << "Da = " << Da << std::endl;
        std::exit(EXIT_FAILURE);
    }

    if (odometry_options.state_options.imu_model == ImuModel::KALIBR)
    {
        odometry_options.vec_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2);
        odometry_options.vec_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2);
    }
    else
    {
        odometry_options.vec_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2);
        odometry_options.vec_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2);
    }

    odometry_options.vec_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2);
    odometry_options.q_imu_acc = quatType::rotToQuat(R_imu_acc);
    odometry_options.q_imu_gyr = quatType::rotToQuat(R_imu_gyr);

    nh.param<double>("odometry_parameter/dt_slam_delay", para_double, 2.0); odometry_options.dt_slam_delay = para_double;

    odometry_options.imu_noises.sigma_w = odometry_options.init_options.sigma_w;
    odometry_options.imu_noises.sigma_wb = odometry_options.init_options.sigma_wb;
    odometry_options.imu_noises.sigma_a = odometry_options.init_options.sigma_a;
    odometry_options.imu_noises.sigma_ab = odometry_options.init_options.sigma_ab;
    odometry_options.imu_noises.sigma_w_2 = std::pow(odometry_options.imu_noises.sigma_w, 2);
    odometry_options.imu_noises.sigma_wb_2 = std::pow(odometry_options.imu_noises.sigma_wb, 2);
    odometry_options.imu_noises.sigma_a_2 = std::pow(odometry_options.imu_noises.sigma_a, 2);
    odometry_options.imu_noises.sigma_ab_2 = std::pow(odometry_options.imu_noises.sigma_ab, 2);

    nh.param<double>("odometry_parameter/up_msckf_sigma_px", para_double, 1.0); odometry_options.msckf_options.sigma_pix = para_double;
    nh.param<double>("odometry_parameter/up_msckf_chi2_multipler", para_double, 5.0); odometry_options.msckf_options.chi2_multipler = para_double;
    nh.param<double>("odometry_parameter/up_slam_sigma_px", para_double, 1.0); odometry_options.slam_options.sigma_pix = para_double;
    nh.param<double>("odometry_parameter/up_slam_chi2_multipler", para_double, 5.0); odometry_options.slam_options.chi2_multipler = para_double;
    odometry_options.msckf_options.sigma_pix_sq = std::pow(odometry_options.msckf_options.sigma_pix, 2);
    odometry_options.slam_options.sigma_pix_sq = std::pow(odometry_options.slam_options.sigma_pix, 2);

    nh.param<bool>("odometry_parameter/downsample_cameras", para_bool, false); odometry_options.downsample_cameras = para_bool;
    nh.param<int>("odometry_parameter/num_pts", para_int, 150); odometry_options.num_pts = para_int;
    nh.param<int>("odometry_parameter/fast_threshold", para_int, 20); odometry_options.fast_threshold = para_int;
    nh.param<int>("odometry_parameter/patch_size_x", para_int, 5); odometry_options.patch_size_x = para_int;
    nh.param<int>("odometry_parameter/patch_size_y", para_int, 5); odometry_options.patch_size_y = para_int;
    nh.param<int>("odometry_parameter/min_px_dist", para_int, 10); odometry_options.min_px_dist = para_int;
    nh.param<std::string>("odometry_parameter/histogram_method", str_temp, "histogram");
    if (str_temp == "none") odometry_options.histogram_method = HistogramMethod::NONE;
    else if (str_temp == "histogram") odometry_options.histogram_method = HistogramMethod::HISTOGRAM;
    else if (str_temp == "clahe") odometry_options.histogram_method = HistogramMethod::CLAHE;
    else {
        std::cout << "Invalid feature histogram specified: " << str_temp << std::endl;
        std::cout << "Please select a valid histogram method: none, histogram, clahe." << std::endl;
        std::exit(EXIT_FAILURE);
    }
    nh.param<double>("odometry_parameter/track_frequency", para_double, 20.0); odometry_options.track_frequency = para_double;

    nh.param<bool>("odometry_parameter/use_huber", para_bool, true); odometry_options.state_options.use_huber = para_bool;
    nh.param<bool>("odometry_parameter/use_keyframe", para_bool, true); odometry_options.use_keyframe = para_bool;

    nh.param<bool>("feature_parameter/refine_features", para_bool, true); odometry_options.featinit_options.refine_features = para_bool;
    nh.param<int>("feature_parameter/max_runs", para_int, 5); odometry_options.featinit_options.max_runs = para_int;
    nh.param<double>("feature_parameter/init_lamda", para_double, 1e-3); odometry_options.featinit_options.init_lamda = para_double;
    nh.param<double>("feature_parameter/max_lamda", para_double, 1e10); odometry_options.featinit_options.max_lamda = para_double;
    nh.param<double>("feature_parameter/min_dx", para_double, 1e-6); odometry_options.featinit_options.min_dx = para_double;
    nh.param<double>("feature_parameter/min_dcost", para_double, 1e-6); odometry_options.featinit_options.min_dcost = para_double;
    nh.param<double>("feature_parameter/lam_mult", para_double, 10.0); odometry_options.featinit_options.lam_mult = para_double;
    nh.param<double>("feature_parameter/min_dist", para_double, 0.10); odometry_options.featinit_options.min_dist = para_double;
    nh.param<double>("feature_parameter/max_dist", para_double, 60.0); odometry_options.featinit_options.max_dist = para_double;
    nh.param<double>("feature_parameter/max_baseline", para_double, 40.0); odometry_options.featinit_options.max_baseline = para_double;
    nh.param<double>("feature_parameter/max_cond_number", para_double, 10000.0); odometry_options.featinit_options.max_cond_number = para_double;

    nh.param<int>("feature_parameter/keyframe_parallax", para_int, 10); setting_min_parallax = para_int;
    setting_min_parallax = setting_min_parallax / cam_calib_1_left.at(0);

    nh.param<double>("voxel_parameter/voxel_size", para_double, 0.1); odometry_options.voxel_size = odometry_options.state_options.voxel_size = para_double;
    nh.param<int>("voxel_parameter/max_num_points_in_voxel", para_int, 5); odometry_options.max_num_points_in_voxel = odometry_options.state_options.max_num_points_in_voxel = para_int;
    nh.param<double>("voxel_parameter/min_distance_points", para_double, 0.03); odometry_options.min_distance_points = odometry_options.state_options.min_distance_points = para_double;
    nh.param<int>("voxel_parameter/nb_voxels_visited", para_int, 1); odometry_options.nb_voxels_visited = para_int;
    nh.param<bool>("voxel_parameter/use_all_points", para_bool, false); odometry_options.use_all_points = para_bool;
}
    void allocateMemory() {
    state_ptr = std::make_shared<state>(odometry_options.state_options);

    state_ptr->calib_imu_dw->setValue(odometry_options.vec_dw);
    state_ptr->calib_imu_dw->setFej(odometry_options.vec_dw);
    state_ptr->calib_imu_da->setValue(odometry_options.vec_da);
    state_ptr->calib_imu_da->setFej(odometry_options.vec_da);
    state_ptr->calib_imu_tg->setValue(odometry_options.vec_tg);
    state_ptr->calib_imu_tg->setFej(odometry_options.vec_tg);
    state_ptr->calib_imu_gyr->setValue(odometry_options.q_imu_gyr);
    state_ptr->calib_imu_gyr->setFej(odometry_options.q_imu_gyr);
    state_ptr->calib_imu_acc->setValue(odometry_options.q_imu_acc);
    state_ptr->calib_imu_acc->setFej(odometry_options.q_imu_acc);

    Eigen::VectorXd temp_camimu_dt;
    temp_camimu_dt.resize(1);
    temp_camimu_dt(0) = odometry_options.calib_camimu_dt;
    state_ptr->calib_dt_imu_cam->setValue(temp_camimu_dt);
    state_ptr->calib_dt_imu_cam->setFej(temp_camimu_dt);

    state_ptr->cam_intrinsics_cameras = odometry_options.camera_intrinsics;

    for (int i = 0; i < 2; i++)
    {
        state_ptr->cam_intrinsics.at(i)->setValue(odometry_options.camera_intrinsics.at(i)->getValue());
        state_ptr->cam_intrinsics.at(i)->setFej(odometry_options.camera_intrinsics.at(i)->getValue());
        state_ptr->calib_cam_imu.at(i)->setValue(odometry_options.camera_extrinsics.at(i));
        state_ptr->calib_cam_imu.at(i)->setFej(odometry_options.camera_extrinsics.at(i));
    }

    int init_max_features = std::floor((double)odometry_options.init_options.init_max_features / (double)2.0);

    featureTracker = std::shared_ptr<trackKLT>(new trackKLT(state_ptr->cam_intrinsics_cameras, init_max_features, odometry_options.histogram_method,
        odometry_options.fast_threshold, odometry_options.patch_size_x, odometry_options.patch_size_y, odometry_options.min_px_dist));

    propagator_ptr = std::make_shared<propagator>(odometry_options.imu_noises, odometry_options.gravity_mag);

    initializer_ptr = std::make_shared<inertialInitializer>(odometry_options.init_options, featureTracker->getFeatureDatabase());

    updaterMsckf_ptr = std::make_shared<updaterMsckf>(odometry_options.msckf_options, odometry_options.featinit_options);

    updaterSlam_ptr = std::make_shared<updaterSlam>(odometry_options.slam_options, odometry_options.featinit_options, featureTracker->getFeatureDatabase());

    gammaPixel_ptr = std::make_shared<gammaPixel>();

    int wlvl = wG[0], hlvl = hG[0];

    while(wlvl % 2 == 0 && hlvl % 2 == 0 && wlvl * hlvl > 5000 && pyr_levels_used < PYR_LEVELS)
    {
        wlvl /=2;
        hlvl /=2;
        pyr_levels_used++;
    }

    for (int level = 1; level < pyr_levels_used; ++ level)
    {
        wG[level] = wG[0] >> level;
        hG[level] = hG[0] >> level;
    }

    frame_count = 0;
}
    void initialValue() {
    last_time_image = -1;

    last_imu_data.timestamp = -1;
    last_imu_data.gyr.setZero();
    last_imu_data.acc.setZero();

    startup_time = -1;
    current_time = -1;

    time_newest_imu = -1;
}
    void imuHandler(const sensor_msgs::Imu::ConstPtr &msg) {
    imuData imu_data;
    imu_data.timestamp = msg->header.stamp.toSec();
    imu_data.gyr << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
    imu_data.acc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;

    processImu(imu_data);

    time_newest_imu = imu_data.timestamp;
}
    void stereoImageHandler(const sensor_msgs::ImageConstPtr &msg_0, const sensor_msgs::ImageConstPtr &msg_1, int cam_0, int cam_1) {
    double timestamp = msg_0->header.stamp.toSec();
    double time_delta = 1.0 / odometry_options.track_frequency;

    if (camera_last_timestamp.find(cam_0) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_0) + time_delta) return;

    camera_last_timestamp[cam_0] = timestamp;

    cv_bridge::CvImageConstPtr cv_ptr_0;
    try
    {
        cv_ptr_0 = cv_bridge::toCvShare(msg_0, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception &e)
    {
        std::cout << "cv_bridge exception: " << e.what() << std::endl;
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptr_1;
    try
    {
        cv_ptr_1 = cv_bridge::toCvShare(msg_1, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception &e)
    {
        std::cout << "cv_bridge exception: " << e.what() << std::endl;
        return;
    }

    cameraData camera_data;
    camera_data.timestamp = cv_ptr_0->header.stamp.toSec();
    camera_data.camera_ids.push_back(cam_0);
    camera_data.camera_ids.push_back(cam_1);
    camera_data.images.push_back(cv_ptr_0->image.clone());
    camera_data.images.push_back(cv_ptr_1->image.clone());

    if (odometry_options.use_mask)
    {
        camera_data.masks.push_back(odometry_options.masks.at(cam_0));
        camera_data.masks.push_back(odometry_options.masks.at(cam_1));
    }
    else
    {
        camera_data.masks.push_back(cv::Mat::zeros(cv_ptr_0->image.rows, cv_ptr_0->image.cols, CV_8UC1));
        camera_data.masks.push_back(cv::Mat::zeros(cv_ptr_1->image.rows, cv_ptr_1->image.cols, CV_8UC1));
    }

    camera_buffer.push(camera_data);
}
    void run() {
    double timestamp_imu_in_camera = time_newest_imu - state_ptr->calib_dt_imu_cam->value()(0);

    while (!camera_buffer.empty() && camera_buffer.front().timestamp < timestamp_imu_in_camera)
    {
        // time test
        boost::posix_time::ptime rT_begin_sum = boost::posix_time::microsec_clock::local_time();
        // time test

        processImage(camera_buffer.front());

        // time test
        boost::posix_time::ptime rT_end_sum = boost::posix_time::microsec_clock::local_time();
        double time_cost = (rT_end_sum - rT_begin_sum).total_microseconds() * 1e-6;
        sum_time_sum = sum_time_sum * n_time_sum + time_cost;
        n_time_sum++;
        sum_time_sum = sum_time_sum / n_time_sum;
        std::cout << std::fixed << std::setprecision(6) << "[total] time cost = " << sum_time_sum << std::endl;
        // time test

        camera_buffer.pop();
    }
}
    ros::NodeHandle nh;
    std::string image_left_topic;
    std::string image_right_topic;
    std::string imu_topic;
    ros::Subscriber sub_imu;
    std::vector<ros::Subscriber> subs_cam;
    std::vector<std::shared_ptr<message_filters::Synchronizer<SyncStereoImage>>> sync_cam;
    std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>>> sync_subs_cam;
    void pubFeatImage(cv::Mat &stereo_image, double &timestamp) {
    if (!stereo_image.empty() && pub_feat_image.getNumSubscribers() > 0)
    {
        try {
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", stereo_image).toImageMsg();
            msg->header.stamp = ros::Time().fromSec(timestamp);
            msg->header.frame_id = "camera_init";
            pub_feat_image.publish(msg);
        }
        catch (cv_bridge::Exception& e) {
            ROS_ERROR("cv_bridge exception: %s", e.what());
        }
    }
}
    void pubOdometry(std::shared_ptr<state> state_ptr, double &timestamp) {
    if (pub_odom.getNumSubscribers() > 0)
    {
        odom.header.frame_id = "camera_init";
        odom.child_frame_id = "body";
        odom.header.stamp = ros::Time().fromSec(timestamp);
        odom.pose.pose.orientation.x = state_ptr->imu_ptr->getQuat()(0);
        odom.pose.pose.orientation.y = state_ptr->imu_ptr->getQuat()(1);
        odom.pose.pose.orientation.z = state_ptr->imu_ptr->getQuat()(2);
        odom.pose.pose.orientation.w = state_ptr->imu_ptr->getQuat()(3);
        odom.pose.pose.position.x = state_ptr->imu_ptr->getPos()(0);
        odom.pose.pose.position.y = state_ptr->imu_ptr->getPos()(1);
        odom.pose.pose.position.z = state_ptr->imu_ptr->getPos()(2);
        pub_odom.publish(odom);
    }
}
    void setPoseStamp(geometry_msgs::PoseStamped &body_pose_out, std::shared_ptr<state> state_ptr) {
    body_pose_out.pose.position.x = state_ptr->imu_ptr->getPos()(0);
    body_pose_out.pose.position.y = state_ptr->imu_ptr->getPos()(1);
    body_pose_out.pose.position.z = state_ptr->imu_ptr->getPos()(2);

    body_pose_out.pose.orientation.x = state_ptr->imu_ptr->getQuat()(0);
    body_pose_out.pose.orientation.y = state_ptr->imu_ptr->getQuat()(1);
    body_pose_out.pose.orientation.z = state_ptr->imu_ptr->getQuat()(2);
    body_pose_out.pose.orientation.w = state_ptr->imu_ptr->getQuat()(3);
}
    void pubPath(std::shared_ptr<state> state_ptr, double &timestamp) {
    if (pub_path.getNumSubscribers() > 0)
    {
        setPoseStamp(msg_body_pose, state_ptr);
        msg_body_pose.header.stamp = ros::Time().fromSec(timestamp);
        msg_body_pose.header.frame_id = "camera_init";

        static int i = 0;
        i++;
        if (i % 10 == 0)
        {
            path.poses.push_back(msg_body_pose);
            path.header.stamp = ros::Time().fromSec(timestamp);
            path.header.frame_id ="camera_init";
            pub_path.publish(path);
        }
    }
}
    void pubHistoryPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_history, double &timestamp) {
    if (pub_points_history.getNumSubscribers() > 0)
    {
        sensor_msgs::PointCloud2 point_cloud_msg;
        pcl::toROSMsg(*points_history, point_cloud_msg);
        point_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        point_cloud_msg.header.frame_id = "camera_init";
        pub_points_history.publish(point_cloud_msg);
    }
}
    void pubWindowPoints(std::shared_ptr<state> state_ptr, double &timestamp) {
    points_window->clear();

    if (pub_points_window.getNumSubscribers() > 0)
    {
        auto it0 = state_ptr->map_points.begin();
        while (it0 != state_ptr->map_points.end())
        {
            pcl::PointXYZRGB point_temp;
            point_temp.x = (*it0).second->getPointXYZ(false)[0];
            point_temp.y = (*it0).second->getPointXYZ(false)[1];
            point_temp.z = (*it0).second->getPointXYZ(false)[2];
            point_temp.r = (*it0).second->color;
            point_temp.g = (*it0).second->color;
            point_temp.b = (*it0).second->color;
            points_window->points.push_back(point_temp);

            it0++;
        }

        sensor_msgs::PointCloud2 point_cloud_msg;
        pcl::toROSMsg(*points_window, point_cloud_msg);
        point_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        point_cloud_msg.header.frame_id = "camera_init";
        pub_points_window.publish(point_cloud_msg);
    }
}
    void pubHistoryVoxels(pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_history, double &timestamp) {
    if (pub_voxels_history.getNumSubscribers() > 0)
    {
        sensor_msgs::PointCloud2 point_cloud_msg;
        pcl::toROSMsg(*voxels_history, point_cloud_msg);
        point_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        point_cloud_msg.header.frame_id = "camera_init";
        pub_voxels_history.publish(point_cloud_msg);
    }

    voxels_history->clear();
}
    void pubVisitVoxels(pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_visit, double &timestamp) {
    if (pub_voxels_visit.getNumSubscribers() > 0)
    {
        sensor_msgs::PointCloud2 point_cloud_msg;
        pcl::toROSMsg(*voxels_visit, point_cloud_msg);
        point_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        point_cloud_msg.header.frame_id = "camera_init";
        pub_voxels_visit.publish(point_cloud_msg);
    }
}
    image_transport::ImageTransport it;
    image_transport::Publisher pub_feat_image;
    ros::Publisher pub_odom;
    ros::Publisher pub_path;
    ros::Publisher pub_points_history;
    ros::Publisher pub_points_window;
    ros::Publisher pub_voxels_history;
    ros::Publisher pub_voxels_visit;
    geometry_msgs::PoseStamped msg_body_pose;
    nav_msgs::Odometry odom;
    nav_msgs::Path path;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_history;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_window;
    pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_history;
    pcl::PointCloud<pcl::PointXYZI>::Ptr voxels_visit;
};

class baseType {
  public:
    struct updaterHelperFeature
	{
		size_t feature_id;

		std::unordered_map<size_t, std::vector<Eigen::VectorXf>> uvs;

		std::unordered_map<size_t, std::vector<Eigen::VectorXf>> uvs_norm;

		std::unordered_map<size_t, std::vector<double>> timestamps;

		std::unordered_map<size_t, std::vector<std::shared_ptr<frame>>> frames;

		std::unordered_map<size_t, std::vector<float>> color;

		int anchor_cam_id = -1;

		double anchor_clone_timestamp = -1;

		Eigen::Vector3d position_anchor;

		Eigen::Vector3d position_anchor_fej;

		Eigen::Vector3d position_global;

		Eigen::Vector3d position_global_fej;
	};
    static void getFeatureJacobianRepresentation(std::shared_ptr<state> state_ptr, updaterHelperFeature &feature, Eigen::MatrixXd &H_f,
		std::vector<Eigen::MatrixXd> &H_x, std::vector<std::shared_ptr<baseType>> &x_order) {
	H_f.resize(3, 3);
	H_f.setIdentity();
	return;
}
    static void getFeatureJacobianFull(std::shared_ptr<state> state_ptr, updaterHelperFeature &feature, Eigen::MatrixXd &H_f,
		Eigen::MatrixXd &H_x, Eigen::VectorXd &res, std::vector<std::shared_ptr<baseType>> &x_order) {
	int total_meas = 0;
	for (auto const &pair : feature.timestamps)
	{
		total_meas += (int)pair.second.size();
	}

	int total_hx = 0;
	std::unordered_map<std::shared_ptr<baseType>, size_t> map_hx;
	for (auto const &pair : feature.timestamps)
	{
		std::shared_ptr<poseJpl> calibration = state_ptr->calib_cam_imu.at(pair.first);
		std::shared_ptr<vec> distortion = state_ptr->cam_intrinsics.at(pair.first);

		if (state_ptr->options.do_calib_camera_pose)
		{
			map_hx.insert({calibration, total_hx});
			x_order.push_back(calibration);
			total_hx += calibration->getSize();
		}

		if (state_ptr->options.do_calib_camera_intrinsics)
		{
			map_hx.insert({distortion, total_hx});
			x_order.push_back(distortion);
			total_hx += distortion->getSize();
		}

		for (size_t m = 0; m < feature.timestamps[pair.first].size(); m++)
		{
			std::shared_ptr<poseJpl> clone_Ci = state_ptr->clones_imu.at(feature.timestamps[pair.first].at(m));

			if (map_hx.find(clone_Ci) == map_hx.end())
			{
				map_hx.insert({clone_Ci, total_hx});
				x_order.push_back(clone_Ci);
				total_hx += clone_Ci->getSize();
			}
		}
	}

	Eigen::Vector3d p_FinG = feature.position_global;
	Eigen::Vector3d p_FinG_fej = feature.position_global_fej;

	int c = 0;
	int jacobsize = 3;

	res = Eigen::VectorXd::Zero(2 * total_meas);
	H_f = Eigen::MatrixXd::Zero(2 * total_meas, jacobsize);
	H_x = Eigen::MatrixXd::Zero(2 * total_meas, total_hx);

	Eigen::MatrixXd dpfg_dlambda;
	std::vector<Eigen::MatrixXd> dpfg_dx;
	std::vector<std::shared_ptr<baseType>> dpfg_dx_order;
	updaterHelper::getFeatureJacobianRepresentation(state_ptr, feature, dpfg_dlambda, dpfg_dx, dpfg_dx_order);

	for (auto const &pair : feature.timestamps)
	{
		std::shared_ptr<vec> distortion = state_ptr->cam_intrinsics.at(pair.first);
		std::shared_ptr<poseJpl> calibration = state_ptr->calib_cam_imu.at(pair.first);
		Eigen::Matrix3d R_ItoC = calibration->getRot();
		Eigen::Vector3d p_IinC = calibration->getPos();

		for (size_t m = 0; m < feature.timestamps[pair.first].size(); m++)
		{
			std::shared_ptr<poseJpl> clone_Ii = state_ptr->clones_imu.at(feature.timestamps[pair.first].at(m));
			Eigen::Matrix3d R_GtoIi = clone_Ii->getRot();
			Eigen::Vector3d p_IiinG = clone_Ii->getPos();

			Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);

			Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;
			Eigen::Vector2d uv_norm;
			uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

			Eigen::Vector2d uv_dist;
			uv_dist = state_ptr->cam_intrinsics_cameras.at(pair.first)->distortD(uv_norm);

			Eigen::Vector2d uv_m;
			uv_m << (double)feature.uvs[pair.first].at(m)(0), (double)feature.uvs[pair.first].at(m)(1);
			res.block(2 * c, 0, 2, 1) = uv_m - uv_dist;

			if (state_ptr->options.do_fej)
			{
				R_GtoIi = clone_Ii->getRotFej();
				p_IiinG = clone_Ii->getPosFej();

				p_FinIi = R_GtoIi * (p_FinG_fej - p_IiinG);
				p_FinCi = R_ItoC * p_FinIi + p_IinC;
			}

			Eigen::MatrixXd dz_dzn, dz_dzeta;
			state_ptr->cam_intrinsics_cameras.at(pair.first)->computeDistortJacobian(uv_norm, dz_dzn, dz_dzeta);

			Eigen::MatrixXd dzn_dpfc = Eigen::MatrixXd::Zero(2, 3);
			dzn_dpfc << 1 / p_FinCi(2), 0, -p_FinCi(0) / (p_FinCi(2) * p_FinCi(2)), 0, 1 / p_FinCi(2), -p_FinCi(1) / (p_FinCi(2) * p_FinCi(2));

			Eigen::MatrixXd dpfc_dpfg = R_ItoC * R_GtoIi;

			Eigen::MatrixXd dpfc_dclone = Eigen::MatrixXd::Zero(3, 6);
			dpfc_dclone.block(0, 0, 3, 3).noalias() = R_ItoC * quatType::skewSymmetric(p_FinIi);
			dpfc_dclone.block(0, 3, 3, 3) = - dpfc_dpfg;

			Eigen::MatrixXd dz_dpfc = dz_dzn * dzn_dpfc;
			Eigen::MatrixXd dz_dpfg = dz_dpfc * dpfc_dpfg;

			H_f.block(2 * c, 0, 2, H_f.cols()).noalias() = dz_dpfg * dpfg_dlambda;
			H_x.block(2 * c, map_hx[clone_Ii], 2, clone_Ii->getSize()).noalias() = dz_dpfc * dpfc_dclone;

			for (size_t i = 0; i < dpfg_dx_order.size(); i++)
			{
				H_x.block(2 * c, map_hx[dpfg_dx_order.at(i)], 2, dpfg_dx_order.at(i)->getSize()).noalias() += dz_dpfg * dpfg_dx.at(i);
			}

			if (state_ptr->options.do_calib_camera_pose)
			{
				Eigen::MatrixXd dpfc_dcalib = Eigen::MatrixXd::Zero(3, 6);
				dpfc_dcalib.block(0, 0, 3, 3) = quatType::skewSymmetric(p_FinCi - p_IinC);
				dpfc_dcalib.block(0, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

				H_x.block(2 * c, map_hx[calibration], 2, calibration->getSize()).noalias() += dz_dpfc * dpfc_dcalib;
			}

			if (state_ptr->options.do_calib_camera_intrinsics)
			{
				H_x.block(2 * c, map_hx[distortion], 2, distortion->getSize()) = dz_dzeta;
			}

			if (state_ptr->options.use_huber)
			{
				double loss = res.block(2 * c, 0, 2, 1).norm();
				double hw = loss < setting_huber_th ? 1 : setting_huber_th / loss;

				if (hw < 1) hw = sqrt(hw);

				res.block(2 * c, 0, 2, 1) *= hw;
				H_x.block(2 * c, 0, 2, H_x.cols()) *= hw;
				H_f.block(2 * c, 0, 2, H_f.cols()) *= hw;
			}

			c++;
		}
	}
}
    static void nullspaceProjectInplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {
	Eigen::JacobiRotation<double> Ho_GR_temp;

	for (int n = 0; n < H_f.cols(); ++n)
	{
		for (int m = (int)H_f.rows() - 1; m > n; m--)
		{
			Ho_GR_temp.makeGivens(H_f(m - 1, n), H_f(m, n));
			(H_f.block(m - 1, n, 2, H_f.cols() - n)).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
			(H_x.block(m - 1, 0, 2, H_x.cols())).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
			(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
		}
	}

	H_x = H_x.block(H_f.cols(), 0, H_x.rows() - H_f.cols(), H_x.cols()).eval();
	res = res.block(H_f.cols(), 0, res.rows() - H_f.cols(), res.cols()).eval();

	assert(H_x.rows() == res.rows());
}
    static void measurementCompressInplace(Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {
	if (H_x.rows() <= H_x.cols())
		return;

	Eigen::JacobiRotation<double> Ho_GR_temp;
	for (int n = 0; n < H_x.cols(); n++)
	{
		for (int m = (int)H_x.rows() - 1; m > n; m--)
		{
			Ho_GR_temp.makeGivens(H_x(m - 1, n), H_x(m, n));

			(H_x.block(m - 1, n, 2, H_x.cols() - n)).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
			(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, Ho_GR_temp.adjoint());
		}
	}

	int r = std::min(H_x.rows(), H_x.cols());

	assert(r <= H_x.rows());
	H_x.conservativeResize(r, H_x.cols());
	res.conservativeResize(r, res.cols());
}
};

class mapPoint {
  public:
    baseType(int size_) {
	size = size_;
}
    virtual void setLocalId(int new_id) {
	id = new_id;
	pose_->setLocalId(new_id);
	v_->setLocalId(pose_->getId() + ((new_id != -1) ? pose_->getSize() : 0));
	bg_->setLocalId(v_->getId() + ((new_id != -1) ? v_->getSize() : 0));
	ba_->setLocalId(bg_->getId() + ((new_id != -1) ? bg_->getSize() : 0));
}
    int getId() {
	return id;
}
    int getSize() {
	return size;
}
    virtual void update(const Eigen::VectorXd &dx) = 0 {
	assert(dx.rows() == size);

	Eigen::Matrix<double, 16, 1> new_x = value_;

	Eigen::Matrix<double, 4, 1> dq;
	dq << 0.5 * dx.block<3, 1>(0, 0), 1.0;
	dq = quatType::quatNorm(dq);

	new_x.block<4, 1>(0, 0) = quatType::quatMultiply(dq, getQuat());
	new_x.block<3, 1>(4, 0) += dx.block<3, 1>(3, 0);

	new_x.block<3, 1>(7, 0) += dx.block<3, 1>(6, 0);
	new_x.block<3, 1>(10, 0) += dx.block<3, 1>(9, 0);
	new_x.block<3, 1>(13, 0) += dx.block<3, 1>(12, 0);

	setValue(new_x);
}
    virtual void setValue(const Eigen::MatrixXd &new_value) {
	setValueInternal(new_value);
}
    virtual void setFej(const Eigen::MatrixXd &new_value) {
	setFejInternal(new_value);
}
    virtual std::shared_ptr<baseType> clone() = 0 {
	auto clone_temp = std::shared_ptr<baseType>(new imuState());
	clone_temp->setValue(value());
	clone_temp->setFej(fej());
	return clone_temp;
}
    virtual std::shared_ptr<baseType> checkIfSubvariable(const std::shared_ptr<baseType> check) {
	if (check == pose_)
	{
		return pose_;
	}
	else if (check == pose_->checkIfSubvariable(check))
	{
		return pose_->checkIfSubvariable(check);
	}
	else if (check == v_)
	{
		return v_;
	}
	else if (check == bg_)
	{
		return bg_;
	}
	else if (check == ba_)
	{
		return ba_;
	}
	return nullptr;
}
  protected:
    Eigen::MatrixXd fej_;
    Eigen::MatrixXd value_;
    int id = -1;
    int size = -1;
  public:
    jplQuat() {
	Eigen::Vector4d q_temp = Eigen::Vector4d::Zero();
	q_temp(3) = 1.0;
	setValueInternal(q_temp);
	setFejInternal(q_temp);
}
    void update(const Eigen::VectorXd &dx) override {
	assert(dx.rows() == size);

	Eigen::Matrix<double, 16, 1> new_x = value_;

	Eigen::Matrix<double, 4, 1> dq;
	dq << 0.5 * dx.block<3, 1>(0, 0), 1.0;
	dq = quatType::quatNorm(dq);

	new_x.block<4, 1>(0, 0) = quatType::quatMultiply(dq, getQuat());
	new_x.block<3, 1>(4, 0) += dx.block<3, 1>(3, 0);

	new_x.block<3, 1>(7, 0) += dx.block<3, 1>(6, 0);
	new_x.block<3, 1>(10, 0) += dx.block<3, 1>(9, 0);
	new_x.block<3, 1>(13, 0) += dx.block<3, 1>(12, 0);

	setValue(new_x);
}
    void setValue(const Eigen::MatrixXd &new_value) override {
	setValueInternal(new_value);
}
    void setFej(const Eigen::MatrixXd &new_value) override {
	setFejInternal(new_value);
}
    std::shared_ptr<baseType> clone() override {
	auto clone_temp = std::shared_ptr<baseType>(new imuState());
	clone_temp->setValue(value());
	clone_temp->setFej(fej());
	return clone_temp;
}
    Eigen::Matrix<double, 3, 3> getRot() {
	return pose_->getRot();
}
    Eigen::Matrix<double, 3, 3> getRotFej() {
	return pose_->getRotFej();
}
  protected:
    Eigen::Matrix<double, 3, 3> R;
    Eigen::Matrix<double, 3, 3> R_fej;
    void setValueInternal(const Eigen::MatrixXd &new_value) {
	assert(new_value.rows() == 16);
	assert(new_value.cols() == 1);

	pose_->setValue(new_value.block<7, 1>(0, 0));
	v_->setValue(new_value.block<3, 1>(7, 0));
	bg_->setValue(new_value.block<3, 1>(10, 0));
	ba_->setValue(new_value.block<3, 1>(13, 0));

	value_ = new_value;
}
    void setFejInternal(const Eigen::MatrixXd &new_value) {
	assert(new_value.rows() == 16);
	assert(new_value.cols() == 1);

	pose_->setFej(new_value.block<7, 1>(0, 0));
	v_->setFej(new_value.block<3, 1>(7, 0));
	bg_->setFej(new_value.block<3, 1>(10, 0));
	ba_->setFej(new_value.block<3, 1>(13, 0));

	fej_ = new_value;
}
  public:
    vec(int dim) {
	value_ = Eigen::VectorXd::Zero(dim);
	fej_ = Eigen::VectorXd::Zero(dim);
}
    void update(const Eigen::VectorXd &dx) override {
	assert(dx.rows() == size);

	Eigen::Matrix<double, 16, 1> new_x = value_;

	Eigen::Matrix<double, 4, 1> dq;
	dq << 0.5 * dx.block<3, 1>(0, 0), 1.0;
	dq = quatType::quatNorm(dq);

	new_x.block<4, 1>(0, 0) = quatType::quatMultiply(dq, getQuat());
	new_x.block<3, 1>(4, 0) += dx.block<3, 1>(3, 0);

	new_x.block<3, 1>(7, 0) += dx.block<3, 1>(6, 0);
	new_x.block<3, 1>(10, 0) += dx.block<3, 1>(9, 0);
	new_x.block<3, 1>(13, 0) += dx.block<3, 1>(12, 0);

	setValue(new_x);
}
    std::shared_ptr<baseType> clone() override {
	auto clone_temp = std::shared_ptr<baseType>(new imuState());
	clone_temp->setValue(value());
	clone_temp->setFej(fej());
	return clone_temp;
}
  public:
    poseJpl() {
	q_ = std::shared_ptr<jplQuat>(new jplQuat());
	p_ = std::shared_ptr<vec>(new vec(3));

	Eigen::Matrix<double, 7, 1> pose_temp;
	pose_temp.setZero();
    pose_temp(3) = 1.0;
    setValueInternal(pose_temp);
    setFejInternal(pose_temp);
}
    void setLocalId(int new_id) override {
	id = new_id;
	pose_->setLocalId(new_id);
	v_->setLocalId(pose_->getId() + ((new_id != -1) ? pose_->getSize() : 0));
	bg_->setLocalId(v_->getId() + ((new_id != -1) ? v_->getSize() : 0));
	ba_->setLocalId(bg_->getId() + ((new_id != -1) ? bg_->getSize() : 0));
}
    void update(const Eigen::VectorXd &dx) override {
	assert(dx.rows() == size);

	Eigen::Matrix<double, 16, 1> new_x = value_;

	Eigen::Matrix<double, 4, 1> dq;
	dq << 0.5 * dx.block<3, 1>(0, 0), 1.0;
	dq = quatType::quatNorm(dq);

	new_x.block<4, 1>(0, 0) = quatType::quatMultiply(dq, getQuat());
	new_x.block<3, 1>(4, 0) += dx.block<3, 1>(3, 0);

	new_x.block<3, 1>(7, 0) += dx.block<3, 1>(6, 0);
	new_x.block<3, 1>(10, 0) += dx.block<3, 1>(9, 0);
	new_x.block<3, 1>(13, 0) += dx.block<3, 1>(12, 0);

	setValue(new_x);
}
    void setValue(const Eigen::MatrixXd &new_value) override {
	setValueInternal(new_value);
}
    void setFej(const Eigen::MatrixXd &new_value) override {
	setFejInternal(new_value);
}
    std::shared_ptr<baseType> clone() override {
	auto clone_temp = std::shared_ptr<baseType>(new imuState());
	clone_temp->setValue(value());
	clone_temp->setFej(fej());
	return clone_temp;
}
    std::shared_ptr<baseType> checkIfSubvariable(const std::shared_ptr<baseType> check) override {
	if (check == pose_)
	{
		return pose_;
	}
	else if (check == pose_->checkIfSubvariable(check))
	{
		return pose_->checkIfSubvariable(check);
	}
	else if (check == v_)
	{
		return v_;
	}
	else if (check == bg_)
	{
		return bg_;
	}
	else if (check == ba_)
	{
		return ba_;
	}
	return nullptr;
}
    Eigen::Matrix<double, 3, 3> getRot() {
	return pose_->getRot();
}
    Eigen::Matrix<double, 3, 3> getRotFej() {
	return pose_->getRotFej();
}
    Eigen::Matrix<double, 4, 1> getQuat() {
	return pose_->getQuat();
}
    Eigen::Matrix<double, 4, 1> getQuatFej() {
	return pose_->getQuatFej();
}
    Eigen::Matrix<double, 3, 1> getPos() {
	return pose_->getPos();
}
    Eigen::Matrix<double, 3, 1> getPosFej() {
	return pose_->getPosFej();
}
    std::shared_ptr<jplQuat> q() {
	return pose_->q();
}
    std::shared_ptr<vec> p() {
	return pose_->p();
}
  protected:
    std::shared_ptr<jplQuat> q_;
    std::shared_ptr<vec> p_;
    void setValueInternal(const Eigen::MatrixXd &new_value) {
	assert(new_value.rows() == 16);
	assert(new_value.cols() == 1);

	pose_->setValue(new_value.block<7, 1>(0, 0));
	v_->setValue(new_value.block<3, 1>(7, 0));
	bg_->setValue(new_value.block<3, 1>(10, 0));
	ba_->setValue(new_value.block<3, 1>(13, 0));

	value_ = new_value;
}
    void setFejInternal(const Eigen::MatrixXd &new_value) {
	assert(new_value.rows() == 16);
	assert(new_value.cols() == 1);

	pose_->setFej(new_value.block<7, 1>(0, 0));
	v_->setFej(new_value.block<3, 1>(7, 0));
	bg_->setFej(new_value.block<3, 1>(10, 0));
	ba_->setFej(new_value.block<3, 1>(13, 0));

	fej_ = new_value;
}
  public:
    imuState() {
	pose_ = std::shared_ptr<poseJpl>(new poseJpl());
	v_ = std::shared_ptr<vec>(new vec(3));
	bg_ = std::shared_ptr<vec>(new vec(3));
	ba_ = std::shared_ptr<vec>(new vec(3));

	Eigen::VectorXd imu_temp = Eigen::VectorXd::Zero(16, 1);
	imu_temp(3) = 1.0;
	setValueInternal(imu_temp);
	setFejInternal(imu_temp);
}
    void setLocalId(int new_id) override {
	id = new_id;
	pose_->setLocalId(new_id);
	v_->setLocalId(pose_->getId() + ((new_id != -1) ? pose_->getSize() : 0));
	bg_->setLocalId(v_->getId() + ((new_id != -1) ? v_->getSize() : 0));
	ba_->setLocalId(bg_->getId() + ((new_id != -1) ? bg_->getSize() : 0));
}
    void update(const Eigen::VectorXd &dx) override {
	assert(dx.rows() == size);

	Eigen::Matrix<double, 16, 1> new_x = value_;

	Eigen::Matrix<double, 4, 1> dq;
	dq << 0.5 * dx.block<3, 1>(0, 0), 1.0;
	dq = quatType::quatNorm(dq);

	new_x.block<4, 1>(0, 0) = quatType::quatMultiply(dq, getQuat());
	new_x.block<3, 1>(4, 0) += dx.block<3, 1>(3, 0);

	new_x.block<3, 1>(7, 0) += dx.block<3, 1>(6, 0);
	new_x.block<3, 1>(10, 0) += dx.block<3, 1>(9, 0);
	new_x.block<3, 1>(13, 0) += dx.block<3, 1>(12, 0);

	setValue(new_x);
}
    void setValue(const Eigen::MatrixXd &new_value) override {
	setValueInternal(new_value);
}
    void setFej(const Eigen::MatrixXd &new_value) override {
	setFejInternal(new_value);
}
    std::shared_ptr<baseType> clone() override {
	auto clone_temp = std::shared_ptr<baseType>(new imuState());
	clone_temp->setValue(value());
	clone_temp->setFej(fej());
	return clone_temp;
}
    std::shared_ptr<baseType> checkIfSubvariable(const std::shared_ptr<baseType> check) override {
	if (check == pose_)
	{
		return pose_;
	}
	else if (check == pose_->checkIfSubvariable(check))
	{
		return pose_->checkIfSubvariable(check);
	}
	else if (check == v_)
	{
		return v_;
	}
	else if (check == bg_)
	{
		return bg_;
	}
	else if (check == ba_)
	{
		return ba_;
	}
	return nullptr;
}
    Eigen::Matrix<double, 3, 3> getRot() {
	return pose_->getRot();
}
    Eigen::Matrix<double, 3, 3> getRotFej() {
	return pose_->getRotFej();
}
    Eigen::Matrix<double, 4, 1> getQuat() {
	return pose_->getQuat();
}
    Eigen::Matrix<double, 4, 1> getQuatFej() {
	return pose_->getQuatFej();
}
    Eigen::Matrix<double, 3, 1> getPos() {
	return pose_->getPos();
}
    Eigen::Matrix<double, 3, 1> getPosFej() {
	return pose_->getPosFej();
}
    Eigen::Matrix<double, 3, 1> getVel() {
	return v_->value();
}
    Eigen::Matrix<double, 3, 1> getVelFej() {
	return v_->fej();
}
    Eigen::Matrix<double, 3, 1> getBg() {
	return bg_->value();
}
    Eigen::Matrix<double, 3, 1> getBgFej() {
	return bg_->fej();
}
    Eigen::Matrix<double, 3, 1> getBa() {
	return ba_->value();
}
    Eigen::Matrix<double, 3, 1> getBaFej() {
	return ba_->fej();
}
    std::shared_ptr<poseJpl> pose() {
	return pose_;
}
    std::shared_ptr<jplQuat> q() {
	return pose_->q();
}
    std::shared_ptr<vec> p() {
	return pose_->p();
}
    std::shared_ptr<vec> v() {
	return v_;
}
    std::shared_ptr<vec> bg() {
	return bg_;
}
    std::shared_ptr<vec> ba() {
	return ba_;
}
  protected:
    std::shared_ptr<poseJpl> pose_;
    std::shared_ptr<vec> v_;
    std::shared_ptr<vec> bg_;
    std::shared_ptr<vec> ba_;
    void setValueInternal(const Eigen::MatrixXd &new_value) {
	assert(new_value.rows() == 16);
	assert(new_value.cols() == 1);

	pose_->setValue(new_value.block<7, 1>(0, 0));
	v_->setValue(new_value.block<3, 1>(7, 0));
	bg_->setValue(new_value.block<3, 1>(10, 0));
	ba_->setValue(new_value.block<3, 1>(13, 0));

	value_ = new_value;
}
    void setFejInternal(const Eigen::MatrixXd &new_value) {
	assert(new_value.rows() == 16);
	assert(new_value.cols() == 1);

	pose_->setFej(new_value.block<7, 1>(0, 0));
	v_->setFej(new_value.block<3, 1>(7, 0));
	bg_->setFej(new_value.block<3, 1>(10, 0));
	ba_->setFej(new_value.block<3, 1>(13, 0));

	fej_ = new_value;
}
  public:
    state(stateOptions &options_) {
	options = options_;

	int current_id = 0;
	imu_ptr = std::make_shared<imuState>();
	imu_ptr->setLocalId(current_id);
	variables.push_back(imu_ptr);
	current_id += imu_ptr->getSize();

	calib_imu_dw = std::make_shared<vec>(6);
	calib_imu_da = std::make_shared<vec>(6);

	if (options.imu_model == ImuModel::KALIBR)
	{
    Eigen::Matrix<double, 6, 1> imu_default = Eigen::Matrix<double, 6, 1>::Zero();
    imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0;
    calib_imu_dw->setValue(imu_default);
    calib_imu_dw->setFej(imu_default);
    calib_imu_da->setValue(imu_default);
    calib_imu_da->setFej(imu_default);
  }
  else
  {
    Eigen::Matrix<double, 6, 1> imu_default = Eigen::Matrix<double, 6, 1>::Zero();
    imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0;
    calib_imu_dw->setValue(imu_default);
    calib_imu_dw->setFej(imu_default);
    calib_imu_da->setValue(imu_default);
    calib_imu_da->setFej(imu_default);
  }
  calib_imu_tg = std::make_shared<vec>(9);
  calib_imu_gyr = std::make_shared<jplQuat>();
  calib_imu_acc = std::make_shared<jplQuat>();

  if (options.do_calib_imu_intrinsics)
  {
    calib_imu_dw->setLocalId(current_id);
    variables.push_back(calib_imu_dw);
    current_id += calib_imu_dw->getSize();

    calib_imu_da->setLocalId(current_id);
    variables.push_back(calib_imu_da);
    current_id += calib_imu_da->getSize();

    if (options.do_calib_imu_g_sensitivity)
    {
      calib_imu_tg->setLocalId(current_id);
      variables.push_back(calib_imu_tg);
      current_id += calib_imu_tg->getSize();
    }

    if (options.imu_model == ImuModel::KALIBR)
    {
      calib_imu_gyr->setLocalId(current_id);
      variables.push_back(calib_imu_gyr);
      current_id += calib_imu_gyr->getSize();
    }
    else
    {
      calib_imu_acc->setLocalId(current_id);
      variables.push_back(calib_imu_acc);
      current_id += calib_imu_acc->getSize();
    }
  }

  calib_dt_imu_cam = std::make_shared<vec>(1);

  if (options.do_calib_camera_timeoffset)
  {
    calib_dt_imu_cam->setLocalId(current_id);
    variables.push_back(calib_dt_imu_cam);
    current_id += calib_dt_imu_cam->getSize();
  }

  for (int i = 0; i < 2; i++)
  {
    auto pose = std::make_shared<poseJpl>();
    auto intrin = std::make_shared<vec>(8);

    calib_cam_imu.insert({i, pose});
    cam_intrinsics.insert({i, intrin});

    if (options.do_calib_camera_pose)
    {
      pose->setLocalId(current_id);
      variables.push_back(pose);
      current_id += pose->getSize();
    }

    if (options.do_calib_camera_intrinsics)
    {
      intrin->setLocalId(current_id);
      variables.push_back(intrin);
      current_id += intrin->getSize();
    }
  }

  cov = std::pow(1e-3, 2) * Eigen::MatrixXd::Identity(current_id, current_id);

  if (options.do_calib_imu_intrinsics)
  {
    cov.block(calib_imu_dw->getId(), calib_imu_dw->getId(), 6, 6) = std::pow(0.005, 2) * Eigen::Matrix<double, 6, 6>::Identity();
    cov.block(calib_imu_da->getId(), calib_imu_da->getId(), 6, 6) = std::pow(0.008, 2) * Eigen::Matrix<double, 6, 6>::Identity();

    if (options.do_calib_imu_g_sensitivity)
      cov.block(calib_imu_tg->getId(), calib_imu_tg->getId(), 9, 9) = std::pow(0.005, 2) * Eigen::Matrix<double, 9, 9>::Identity();

    if (options.imu_model == ImuModel::KALIBR)
      cov.block(calib_imu_gyr->getId(), calib_imu_gyr->getId(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity();
    else
      cov.block(calib_imu_acc->getId(), calib_imu_acc->getId(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity();
  }

  if (options.do_calib_camera_timeoffset)
    cov(calib_dt_imu_cam->getId(), calib_dt_imu_cam->getId()) = std::pow(0.01, 2);

  if (options.do_calib_camera_pose)
  {
    for (int i = 0; i < 2; i++)
    {
      cov.block(calib_cam_imu.at(i)->getId(), calib_cam_imu.at(i)->getId(), 3, 3) = std::pow(0.005, 2) * Eigen::MatrixXd::Identity(3, 3);
      cov.block(calib_cam_imu.at(i)->getId() + 3, calib_cam_imu.at(i)->getId() + 3, 3, 3) = std::pow(0.015, 2) * Eigen::MatrixXd::Identity(3, 3);
    }
  }

  if (options.do_calib_camera_intrinsics)
  {
    for (int i = 0; i < 2; i++) {
      cov.block(cam_intrinsics.at(i)->getId(), cam_intrinsics.at(i)->getId(), 4, 4) = std::pow(1.0, 2) * Eigen::MatrixXd::Identity(4, 4);
      cov.block(cam_intrinsics.at(i)->getId() + 4, cam_intrinsics.at(i)->getId() + 4, 4, 4) = std::pow(0.005, 2) * Eigen::MatrixXd::Identity(4, 4);
    }
  }
}
    double margtimestep() {
	double time = INFINITY;

	for (const auto &clone_imu : clones_imu)
	{
		if (clone_imu.first < time)
		{
			time = clone_imu.first;
		}
	}

  return time;
}
    int maxCovSize() {
	return (int)cov.rows();
}
    static Eigen::Matrix3d Dm(ImuModel imu_model_, const Eigen::MatrixXd &vec_temp) {
	assert(vec_temp.rows() == 6);
	assert(vec_temp.cols() == 1);
	Eigen::Matrix3d D_matrix = Eigen::Matrix3d::Identity();

	if (imu_model_ == ImuModel::KALIBR)
		D_matrix << vec_temp(0), 0, 0, vec_temp(1), vec_temp(3), 0, vec_temp(2), vec_temp(4), vec_temp(5);
	else
		D_matrix << vec_temp(0), vec_temp(1), vec_temp(3), 0, vec_temp(2), vec_temp(4), 0, 0, vec_temp(5);

	return D_matrix;
}
    static Eigen::Matrix3d Tg(const Eigen::MatrixXd &vec_temp) {
	assert(vec_temp.rows() == 9);
	assert(vec_temp.cols() == 1);
	Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero();

	Tg << vec_temp(0), vec_temp(3), vec_temp(6), vec_temp(1), vec_temp(4), vec_temp(7), vec_temp(2), vec_temp(5), vec_temp(8);

	return Tg;
}
    int imuIntrinsicSize() {
	int size_temp = 0;

	if (options.do_calib_imu_intrinsics)
	{
		size_temp += 15;

		if (options.do_calib_imu_g_sensitivity)
			size_temp += 9;
	}

	return size_temp;
}
    double timestamp = -1;
    stateOptions options;
    std::shared_ptr<imuState> imu_ptr;
    std::map<double, std::shared_ptr<poseJpl>> clones_imu;
    std::map<double, std::shared_ptr<frame>> clones_frame;
    std::unordered_map<size_t, std::shared_ptr<mapPoint>> map_points;
    std::shared_ptr<vec> calib_dt_imu_cam;
    std::unordered_map<size_t, std::shared_ptr<poseJpl>> calib_cam_imu;
    std::unordered_map<size_t, std::shared_ptr<vec>> cam_intrinsics;
    std::unordered_map<size_t, std::shared_ptr<cameraBase>> cam_intrinsics_cameras;
    std::shared_ptr<vec> calib_imu_dw;
    std::shared_ptr<vec> calib_imu_da;
    std::shared_ptr<vec> calib_imu_tg;
    std::shared_ptr<jplQuat> calib_imu_gyr;
    std::shared_ptr<jplQuat> calib_imu_acc;
  private:
    friend class stateHelper;
    Eigen::MatrixXd cov;
    std::vector<std::shared_ptr<baseType>> variables;
};

class cameraBase {
  public:
    cameraBase(int width, int height) {

}
    virtual void setValue(const Eigen::MatrixXd &calib) {
	assert(calib.rows() == 8);
	camera_values = calib;

	cv::Matx33d tempK;
	tempK(0, 0) = calib(0);
	tempK(0, 1) = 0;
	tempK(0, 2) = calib(2);
	tempK(1, 0) = 0;
	tempK(1, 1) = calib(1);
	tempK(1, 2) = calib(3);
	tempK(2, 0) = 0;
	tempK(2, 1) = 0;
	tempK(2, 2) = 1;
	camera_k_OPENCV = tempK;

	cv::Vec4d tempD;
	tempD(0) = calib(4);
	tempD(1) = calib(5);
	tempD(2) = calib(6);
	tempD(3) = calib(7);
	camera_d_OPENCV = tempD;
}
    virtual Eigen::Vector2f undistortF(const Eigen::Vector2f &uv_dist) = 0 {
    cv::Matx33d camK = camera_k_OPENCV;
    cv::Vec4d camD = camera_d_OPENCV;

    cv::Mat mat(1, 2, CV_32F);
    mat.at<float>(0, 0) = uv_dist(0);
    mat.at<float>(0, 1) = uv_dist(1);
    mat = mat.reshape(2);

    cv::undistortPoints(mat, mat, camK, camD);

    Eigen::Vector2f pt_out;
    mat = mat.reshape(1);
    pt_out(0) = mat.at<float>(0, 0);
    pt_out(1) = mat.at<float>(0, 1);

    return pt_out;
}
    Eigen::Vector2d undistortD(const Eigen::Vector2d &uv_dist) {
	Eigen::Vector2f ept_1, ept_2;
	ept_1 = uv_dist.cast<float>();
	ept_2 = undistortF(ept_1);

	return ept_2.cast<double>();
}
    cv::Point2f undistortCV(const cv::Point2f &uv_dist) {
	Eigen::Vector2f ept_1, ept_2;
	ept_1 << uv_dist.x, uv_dist.y;
	ept_2 = undistortF(ept_1);
	cv::Point2f pt_out;
	pt_out.x = ept_2(0);
	pt_out.y = ept_2(1);

	return pt_out;
}
    virtual Eigen::Vector2f distortF(const Eigen::Vector2f &uv_norm) = 0 {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;
	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	Eigen::Vector2f uv_dist;
	uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
	uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));

	return uv_dist;
}
    Eigen::Vector2d distortD(const Eigen::Vector2d &uv_norm) {
	Eigen::Vector2f ept_1, ept_2;
	ept_1 = uv_norm.cast<float>();
	ept_2 = distortF(ept_1);

	return ept_2.cast<double>();
}
    cv::Point2f distortCV(const cv::Point2f &uv_norm) {
	Eigen::Vector2f ept_1, ept_2;
	ept_1 << uv_norm.x, uv_norm.y;
	ept_2 = distortF(ept_1);
	cv::Point2f pt_out;
	pt_out.x = ept_2(0);
	pt_out.y = ept_2(1);

	return pt_out;
}
    virtual void computeDistortJacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) = 0 {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;

	H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
	double x = uv_norm(0);
	double y = uv_norm(1);
	double x_2 = uv_norm(0) * uv_norm(0);
	double y_2 = uv_norm(1) * uv_norm(1);
	double x_y = uv_norm(0) * uv_norm(1);
	H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) +
	                             2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x));
	H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) +
	                             2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y));

	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
	H_dz_dzeta(0, 0) = x1;
	H_dz_dzeta(0, 2) = 1;
	H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * r_2;
	H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * r_4;
	H_dz_dzeta(0, 6) = 2 * cam_d(0) * uv_norm(0) * uv_norm(1);
	H_dz_dzeta(0, 7) = cam_d(0) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	H_dz_dzeta(1, 1) = y1;
	H_dz_dzeta(1, 3) = 1;
	H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * r_2;
	H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * r_4;
	H_dz_dzeta(1, 6) = cam_d(1) * (r_2 + 2 * uv_norm(1) * uv_norm(1));
	H_dz_dzeta(1, 7) = 2 * cam_d(1) * uv_norm(0) * uv_norm(1);
}
    Eigen::MatrixXd getValue() {
	return camera_values;
}
    cv::Matx33d getK() {
	return camera_k_OPENCV;
}
    cv::Vec4d getD() {
	return camera_d_OPENCV;
}
    int w() {
	return width;
}
    int h() {
	return height;
}
  protected:
    Eigen::MatrixXd camera_values;
    cv::Matx33d camera_k_OPENCV;
    cv::Vec4d camera_d_OPENCV;
    int width;
    int height;
  public:
    Eigen::Vector2f undistortF(const Eigen::Vector2f &uv_dist) override {
    cv::Matx33d camK = camera_k_OPENCV;
    cv::Vec4d camD = camera_d_OPENCV;

    cv::Mat mat(1, 2, CV_32F);
    mat.at<float>(0, 0) = uv_dist(0);
    mat.at<float>(0, 1) = uv_dist(1);
    mat = mat.reshape(2);

    cv::undistortPoints(mat, mat, camK, camD);

    Eigen::Vector2f pt_out;
    mat = mat.reshape(1);
    pt_out(0) = mat.at<float>(0, 0);
    pt_out(1) = mat.at<float>(0, 1);

    return pt_out;
}
    Eigen::Vector2f distortF(const Eigen::Vector2f &uv_norm) override {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;
	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	Eigen::Vector2f uv_dist;
	uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
	uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));

	return uv_dist;
}
    void computeDistortJacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) override {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;

	H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
	double x = uv_norm(0);
	double y = uv_norm(1);
	double x_2 = uv_norm(0) * uv_norm(0);
	double y_2 = uv_norm(1) * uv_norm(1);
	double x_y = uv_norm(0) * uv_norm(1);
	H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) +
	                             2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x));
	H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) +
	                             2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y));

	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
	H_dz_dzeta(0, 0) = x1;
	H_dz_dzeta(0, 2) = 1;
	H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * r_2;
	H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * r_4;
	H_dz_dzeta(0, 6) = 2 * cam_d(0) * uv_norm(0) * uv_norm(1);
	H_dz_dzeta(0, 7) = cam_d(0) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	H_dz_dzeta(1, 1) = y1;
	H_dz_dzeta(1, 3) = 1;
	H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * r_2;
	H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * r_4;
	H_dz_dzeta(1, 6) = cam_d(1) * (r_2 + 2 * uv_norm(1) * uv_norm(1));
	H_dz_dzeta(1, 7) = 2 * cam_d(1) * uv_norm(0) * uv_norm(1);
}
  public:
    Eigen::Vector2f undistortF(const Eigen::Vector2f &uv_dist) override {
    cv::Matx33d camK = camera_k_OPENCV;
    cv::Vec4d camD = camera_d_OPENCV;

    cv::Mat mat(1, 2, CV_32F);
    mat.at<float>(0, 0) = uv_dist(0);
    mat.at<float>(0, 1) = uv_dist(1);
    mat = mat.reshape(2);

    cv::undistortPoints(mat, mat, camK, camD);

    Eigen::Vector2f pt_out;
    mat = mat.reshape(1);
    pt_out(0) = mat.at<float>(0, 0);
    pt_out(1) = mat.at<float>(0, 1);

    return pt_out;
}
    Eigen::Vector2f distortF(const Eigen::Vector2f &uv_norm) override {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;
	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	Eigen::Vector2f uv_dist;
	uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
	uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));

	return uv_dist;
}
    void computeDistortJacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) override {
	Eigen::MatrixXd cam_d = camera_values;

	double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
	double r_2 = r * r;
	double r_4 = r_2 * r_2;

	H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
	double x = uv_norm(0);
	double y = uv_norm(1);
	double x_2 = uv_norm(0) * uv_norm(0);
	double y_2 = uv_norm(1) * uv_norm(1);
	double x_y = uv_norm(0) * uv_norm(1);
	H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) +
	                             2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x));
	H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
	H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) +
	                             2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y));

	double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
	            cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
	            2 * cam_d(7) * uv_norm(0) * uv_norm(1);

	H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
	H_dz_dzeta(0, 0) = x1;
	H_dz_dzeta(0, 2) = 1;
	H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * r_2;
	H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * r_4;
	H_dz_dzeta(0, 6) = 2 * cam_d(0) * uv_norm(0) * uv_norm(1);
	H_dz_dzeta(0, 7) = cam_d(0) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
	H_dz_dzeta(1, 1) = y1;
	H_dz_dzeta(1, 3) = 1;
	H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * r_2;
	H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * r_4;
	H_dz_dzeta(1, 6) = cam_d(1) * (r_2 + 2 * uv_norm(1) * uv_norm(1));
	H_dz_dzeta(1, 7) = 2 * cam_d(1) * uv_norm(0) * uv_norm(1);
}
};

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_HASH_H
#define TSL_ROBIN_HASH_H

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <exception>
#include <iterator>
#include <limits>
#include <memory>
#include <stdexcept>
#include <tuple>
#include <type_traits>
#include <utility>
#include <vector>
#include "robin_growth_policy.h"

namespace tsl {

namespace detail_robin_hash {

template<typename T>
struct make_void {
    using type = void;
};

template<typename T, typename = void>
struct has_is_transparent: std::false_type {
};

template<typename T>
struct has_is_transparent<T, typename make_void<typename T::is_transparent>::type>: std::true_type {
};

template<typename U>
struct is_power_of_two_policy: std::false_type {
};

template<std::size_t GrowthFactor>
struct is_power_of_two_policy<tsl::rh::power_of_two_growth_policy<GrowthFactor>>: std::true_type {
};

// Only available in C++17, we need to be compatible with C++11
template<class T>
const T& clamp( const T& v, const T& lo, const T& hi) {
    return std::min(hi, std::max(lo, v));
}

template<typename T, typename U>
static T numeric_cast(U value, const char* error_message = "numeric_cast() failed.") {
    T ret = static_cast<T>(value);
    if(static_cast<U>(ret) != value) {
        TSL_RH_THROW_OR_TERMINATE(std::runtime_error, error_message);
    }

    const bool is_same_signedness = (std::is_unsigned<T>::value && std::is_unsigned<U>::value) ||
                                    (std::is_signed<T>::value && std::is_signed<U>::value);
    if(!is_same_signedness && (ret < T{}) != (value < U{})) {
        TSL_RH_THROW_OR_TERMINATE(std::runtime_error, error_message);
    }

    return ret;
}

using truncated_hash_type = std::uint_least32_t;

/**
 * Helper class that stores a truncated hash if StoreHash is true and nothing otherwise.
 */
template<bool StoreHash>
class bucket_entry_hash {
public:
    bool bucket_hash_equal(std::size_t /*hash*/) const noexcept {
        return true;
    }

    truncated_hash_type truncated_hash() const noexcept {
        return 0;
    }

protected:
    void set_hash(truncated_hash_type /*hash*/) noexcept {
    }
};

template<>
class bucket_entry_hash<true> {
public:
    bool bucket_hash_equal(std::size_t hash) const noexcept {
        return m_hash == truncated_hash_type(hash);
    }

    truncated_hash_type truncated_hash() const noexcept {
        return m_hash;
    }

protected:
    void set_hash(truncated_hash_type hash) noexcept {
        m_hash = truncated_hash_type(hash);
    }

private:
    truncated_hash_type m_hash;
};

/**
 * Each bucket entry has:
 * - A value of type `ValueType`.
 * - An integer to store how far the value of the bucket, if any, is from its ideal bucket
 *   (ex: if the current bucket 5 has the value 'foo' and `hash('foo') % nb_buckets` == 3,
 *        `dist_from_ideal_bucket()` will return 2 as the current value of the bucket is two
 *        buckets away from its ideal bucket)
 *   If there is no value in the bucket (i.e. `empty()` is true) `dist_from_ideal_bucket()` will be < 0.
 * - A marker which tells us if the bucket is the last bucket of the bucket array (useful for the
 *   iterator of the hash table).
 * - If `StoreHash` is true, 32 bits of the hash of the value, if any, are also stored in the bucket.
 *   If the size of the hash is more than 32 bits, it is truncated. We don't store the full hash
 *   as storing the hash is a potential opportunity to use the unused space due to the alignment
 *   of the bucket_entry structure. We can thus potentially store the hash without any extra space
 *   (which would not be possible with 64 bits of the hash).
 */
template<typename ValueType, bool StoreHash>
class bucket_entry: public bucket_entry_hash<StoreHash> {
    using bucket_hash = bucket_entry_hash<StoreHash>;

public:
    using value_type = ValueType;
    using distance_type = std::int_least16_t;

    bucket_entry() noexcept: bucket_hash(), m_dist_from_ideal_bucket(EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET),
                             m_last_bucket(false)
    {
        tsl_rh_assert(empty());
    }

    bucket_entry(bool last_bucket) noexcept: bucket_hash(), m_dist_from_ideal_bucket(EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET),
                                             m_last_bucket(last_bucket)
    {
        tsl_rh_assert(empty());
    }

    bucket_entry(const bucket_entry& other) noexcept(std::is_nothrow_copy_constructible<value_type>::value):
            bucket_hash(other),
            m_dist_from_ideal_bucket(EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET),
            m_last_bucket(other.m_last_bucket)
    {
        if(!other.empty()) {
            ::new (static_cast<void*>(std::addressof(m_value))) value_type(other.value());
            m_dist_from_ideal_bucket = other.m_dist_from_ideal_bucket;
        }
    }

    /**
     * Never really used, but still necessary as we must call resize on an empty `std::vector<bucket_entry>`.
     * and we need to support move-only types. See robin_hash constructor for details.
     */
    bucket_entry(bucket_entry&& other) noexcept(std::is_nothrow_move_constructible<value_type>::value):
            bucket_hash(std::move(other)),
            m_dist_from_ideal_bucket(EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET),
            m_last_bucket(other.m_last_bucket)
    {
        if(!other.empty()) {
            ::new (static_cast<void*>(std::addressof(m_value))) value_type(std::move(other.value()));
            m_dist_from_ideal_bucket = other.m_dist_from_ideal_bucket;
        }
    }

    bucket_entry& operator=(const bucket_entry& other)
            noexcept(std::is_nothrow_copy_constructible<value_type>::value)
    {
        if(this != &other) {
            clear();

            bucket_hash::operator=(other);
            if(!other.empty()) {
                ::new (static_cast<void*>(std::addressof(m_value))) value_type(other.value());
            }

            m_dist_from_ideal_bucket = other.m_dist_from_ideal_bucket;
            m_last_bucket = other.m_last_bucket;
        }

        return *this;
    }

    bucket_entry& operator=(bucket_entry&& ) = delete;

    ~bucket_entry() noexcept {
        clear();
    }

    void clear() noexcept {
        if(!empty()) {
            destroy_value();
            m_dist_from_ideal_bucket = EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET;
        }
    }

    bool empty() const noexcept {
        return m_dist_from_ideal_bucket == EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET;
    }

    value_type& value() noexcept {
        tsl_rh_assert(!empty());
        return *reinterpret_cast<value_type*>(std::addressof(m_value));
    }

    const value_type& value() const noexcept {
        tsl_rh_assert(!empty());
        return *reinterpret_cast<const value_type*>(std::addressof(m_value));
    }

    distance_type dist_from_ideal_bucket() const noexcept {
        return m_dist_from_ideal_bucket;
    }

    bool last_bucket() const noexcept {
        return m_last_bucket;
    }

    void set_as_last_bucket() noexcept {
        m_last_bucket = true;
    }

    template<typename... Args>
    void set_value_of_empty_bucket(distance_type dist_from_ideal_bucket,
                                   truncated_hash_type hash, Args&&... value_type_args)
    {
        tsl_rh_assert(dist_from_ideal_bucket >= 0);
        tsl_rh_assert(empty());

        ::new (static_cast<void*>(std::addressof(m_value))) value_type(std::forward<Args>(value_type_args)...);
        this->set_hash(hash);
        m_dist_from_ideal_bucket = dist_from_ideal_bucket;

        tsl_rh_assert(!empty());
    }

    void swap_with_value_in_bucket(distance_type& dist_from_ideal_bucket,
                                   truncated_hash_type& hash, value_type& value)
    {
        tsl_rh_assert(!empty());

        using std::swap;
        swap(value, this->value());
        swap(dist_from_ideal_bucket, m_dist_from_ideal_bucket);

        // Avoid warning of unused variable if StoreHash is false
        (void) hash;
        if(StoreHash) {
            const truncated_hash_type tmp_hash = this->truncated_hash();
            this->set_hash(hash);
            hash = tmp_hash;
        }
    }

    static truncated_hash_type truncate_hash(std::size_t hash) noexcept {
        return truncated_hash_type(hash);
    }

private:
    void destroy_value() noexcept {
        tsl_rh_assert(!empty());
        value().~value_type();
    }

public:
    static const distance_type DIST_FROM_IDEAL_BUCKET_LIMIT = 4096;
    static_assert(DIST_FROM_IDEAL_BUCKET_LIMIT <= std::numeric_limits<distance_type>::max() - 1,
                 "DIST_FROM_IDEAL_BUCKET_LIMIT must be <= std::numeric_limits<distance_type>::max() - 1.");

private:
    using storage = typename std::aligned_storage<sizeof(value_type), alignof(value_type)>::type;

    static const distance_type EMPTY_MARKER_DIST_FROM_IDEAL_BUCKET = -1;

    distance_type m_dist_from_ideal_bucket;
    bool m_last_bucket;
    storage m_value;
};

/**
 * Internal common class used by `robin_map` and `robin_set`.
 *
 * ValueType is what will be stored by `robin_hash` (usually `std::pair<Key, T>` for map and `Key` for set).
 *
 * `KeySelect` should be a `FunctionObject` which takes a `ValueType` in parameter and returns a
 *  reference to the key.
 *
 * `ValueSelect` should be a `FunctionObject` which takes a `ValueType` in parameter and returns a
 *  reference to the value. `ValueSelect` should be void if there is no value (in a set for example).
 *
 * The strong exception guarantee only holds if the expression
 * `std::is_nothrow_swappable<ValueType>::value && std::is_nothrow_move_constructible<ValueType>::value` is true.
 *
 * Behaviour is undefined if the destructor of `ValueType` throws.
 */
template<class ValueType,
         class KeySelect,
         class ValueSelect,
         class Hash,
         class KeyEqual,
         class Allocator,
         bool StoreHash,
         class GrowthPolicy>
class robin_hash: private Hash, private KeyEqual, private GrowthPolicy {
private:
    template<typename U>
    using has_mapped_type = typename std::integral_constant<bool, !std::is_same<U, void>::value>;

    static_assert(noexcept(std::declval<GrowthPolicy>().bucket_for_hash(std::size_t(0))), "GrowthPolicy::bucket_for_hash must be noexcept.");
    static_assert(noexcept(std::declval<GrowthPolicy>().clear()), "GrowthPolicy::clear must be noexcept.");

public:
    template<bool IsConst>
    class robin_iterator;

    using key_type = typename KeySelect::key_type;
    using value_type = ValueType;
    using size_type = std::size_t;
    using difference_type = std::ptrdiff_t;
    using hasher = Hash;
    using key_equal = KeyEqual;
    using allocator_type = Allocator;
    using reference = value_type&;
    using const_reference = const value_type&;
    using pointer = value_type*;
    using const_pointer = const value_type*;
    using iterator = robin_iterator<false>;
    using const_iterator = robin_iterator<true>;

private:
    /**
     * Either store the hash because we are asked by the `StoreHash` template parameter
     * or store the hash because it doesn't cost us anything in size and can be used to speed up rehash.
     */
    static constexpr bool STORE_HASH = StoreHash ||
                                       (
                                         (sizeof(tsl::detail_robin_hash::bucket_entry<value_type, true>) ==
                                          sizeof(tsl::detail_robin_hash::bucket_entry<value_type, false>))
                                         &&
                                         (sizeof(std::size_t) == sizeof(truncated_hash_type) ||
                                          is_power_of_two_policy<GrowthPolicy>::value)
                                         &&
                                          // Don't store the hash for primitive types with default hash.
                                          (!std::is_arithmetic<key_type>::value ||
                                           !std::is_same<Hash, std::hash<key_type>>::value)
                                       );

    /**
     * Only use the stored hash on lookup if we are explicitly asked. We are not sure how slow
     * the KeyEqual operation is. An extra comparison may slow things down with a fast KeyEqual.
     */
    static constexpr bool USE_STORED_HASH_ON_LOOKUP = StoreHash;

    /**
     * We can only use the hash on rehash if the size of the hash type is the same as the stored one or
     * if we use a power of two modulo. In the case of the power of two modulo, we just mask
     * the least significant bytes, we just have to check that the truncated_hash_type didn't truncated
     * more bytes.
     */
    static bool USE_STORED_HASH_ON_REHASH(size_type bucket_count) {
        (void) bucket_count;
        if(STORE_HASH && sizeof(std::size_t) == sizeof(truncated_hash_type)) {
            return true;
        }
        else if(STORE_HASH && is_power_of_two_policy<GrowthPolicy>::value) {
            tsl_rh_assert(bucket_count > 0);
            return (bucket_count - 1) <= std::numeric_limits<truncated_hash_type>::max();
        }
        else {
            return false;
        }
    }

    using bucket_entry = tsl::detail_robin_hash::bucket_entry<value_type, STORE_HASH>;
    using distance_type = typename bucket_entry::distance_type;

    using buckets_allocator = typename std::allocator_traits<allocator_type>::template rebind_alloc<bucket_entry>;
    using buckets_container_type = std::vector<bucket_entry, buckets_allocator>;

public:
    /**
     * The 'operator*()' and 'operator->()' methods return a const reference and const pointer respectively to the
     * stored value type.
     *
     * In case of a map, to get a mutable reference to the value associated to a key (the '.second' in the
     * stored pair), you have to call 'value()'.
     *
     * The main reason for this is that if we returned a `std::pair<Key, T>&` instead
     * of a `const std::pair<Key, T>&`, the user may modify the key which will put the map in a undefined state.
     */
    template<bool IsConst>
    class robin_iterator {
        friend class robin_hash;

    private:
        using bucket_entry_ptr = typename std::conditional<IsConst,
                                                           const bucket_entry*,
                                                           bucket_entry*>::type;

        robin_iterator(bucket_entry_ptr bucket) noexcept: m_bucket(bucket) {
        }

    public:
        using iterator_category = std::forward_iterator_tag;
        using value_type = const typename robin_hash::value_type;
        using difference_type = std::ptrdiff_t;
        using reference = value_type&;
        using pointer = value_type*;

        robin_iterator() noexcept {
        }

        // Copy constructor from iterator to const_iterator.
        template<bool TIsConst = IsConst, typename std::enable_if<TIsConst>::type* = nullptr>
        robin_iterator(const robin_iterator<!TIsConst>& other) noexcept: m_bucket(other.m_bucket) {
        }

        robin_iterator(const robin_iterator& other) = default;
        robin_iterator(robin_iterator&& other) = default;
        robin_iterator& operator=(const robin_iterator& other) = default;
        robin_iterator& operator=(robin_iterator&& other) = default;

        const typename robin_hash::key_type& key() const {
            return KeySelect()(m_bucket->value());
        }

        template<class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value && IsConst>::type* = nullptr>
        const typename U::value_type& value() const {
            return U()(m_bucket->value());
        }

        template<class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value && !IsConst>::type* = nullptr>
        typename U::value_type& value() const {
            return U()(m_bucket->value());
        }

        reference operator*() const {
            return m_bucket->value();
        }

        pointer operator->() const {
            return std::addressof(m_bucket->value());
        }

        robin_iterator& operator++() {
            while(true) {
                if(m_bucket->last_bucket()) {
                    ++m_bucket;
                    return *this;
                }

                ++m_bucket;
                if(!m_bucket->empty()) {
                    return *this;
                }
            }
        }

        robin_iterator operator++(int) {
            robin_iterator tmp(*this);
            ++*this;

            return tmp;
        }

        friend bool operator==(const robin_iterator& lhs, const robin_iterator& rhs) {
            return lhs.m_bucket == rhs.m_bucket;
        }

        friend bool operator!=(const robin_iterator& lhs, const robin_iterator& rhs) {
            return !(lhs == rhs);
        }

    private:
        bucket_entry_ptr m_bucket;
    };

public:
#if defined(__cplusplus) && __cplusplus >= 201402L
    robin_hash(size_type bucket_count,
               const Hash& hash,
               const KeyEqual& equal,
               const Allocator& alloc,
               float min_load_factor = DEFAULT_MIN_LOAD_FACTOR,
               float max_load_factor = DEFAULT_MAX_LOAD_FACTOR):
                                       Hash(hash),
                                       KeyEqual(equal),
                                       GrowthPolicy(bucket_count),
                                       m_buckets_data(
                                           [&]() {
                                               if(bucket_count > max_bucket_count()) {
                                                   TSL_RH_THROW_OR_TERMINATE(std::length_error,
                                                                             "The map exceeds its maximum bucket count.");
                                               }

                                               return bucket_count;
                                           }(), alloc
                                       ),
                                       m_buckets(m_buckets_data.empty()?static_empty_bucket_ptr():m_buckets_data.data()),
                                       m_bucket_count(bucket_count),
                                       m_nb_elements(0),
                                       m_grow_on_next_insert(false),
                                       m_try_shrink_on_next_insert(false)
    {
        if(m_bucket_count > 0) {
            tsl_rh_assert(!m_buckets_data.empty());
            m_buckets_data.back().set_as_last_bucket();
        }

        this->min_load_factor(min_load_factor);
        this->max_load_factor(max_load_factor);
    }
#else
    /**
     * C++11 doesn't support the creation of a std::vector with a custom allocator and 'count' default-inserted elements.
     * The needed contructor `explicit vector(size_type count, const Allocator& alloc = Allocator());` is only
     * available in C++14 and later. We thus must resize after using the `vector(const Allocator& alloc)` constructor.
     *
     * We can't use `vector(size_type count, const T& value, const Allocator& alloc)` as it requires the
     * value T to be copyable.
     */
    robin_hash(size_type bucket_count,
               const Hash& hash,
               const KeyEqual& equal,
               const Allocator& alloc,
               float min_load_factor = DEFAULT_MIN_LOAD_FACTOR,
               float max_load_factor = DEFAULT_MAX_LOAD_FACTOR):
                                       Hash(hash),
                                       KeyEqual(equal),
                                       GrowthPolicy(bucket_count),
                                       m_buckets_data(alloc),
                                       m_buckets(static_empty_bucket_ptr()),
                                       m_bucket_count(bucket_count),
                                       m_nb_elements(0),
                                       m_grow_on_next_insert(false),
                                       m_try_shrink_on_next_insert(false)
    {
        if(bucket_count > max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The map exceeds its maximum bucket count.");
        }

        if(m_bucket_count > 0) {
            m_buckets_data.resize(m_bucket_count);
            m_buckets = m_buckets_data.data();

            tsl_rh_assert(!m_buckets_data.empty());
            m_buckets_data.back().set_as_last_bucket();
        }

        this->min_load_factor(min_load_factor);
        this->max_load_factor(max_load_factor);
    }
#endif

    robin_hash(const robin_hash& other): Hash(other),
                                         KeyEqual(other),
                                         GrowthPolicy(other),
                                         m_buckets_data(other.m_buckets_data),
                                         m_buckets(m_buckets_data.empty()?static_empty_bucket_ptr():m_buckets_data.data()),
                                         m_bucket_count(other.m_bucket_count),
                                         m_nb_elements(other.m_nb_elements),
                                         m_load_threshold(other.m_load_threshold),
                                         m_min_load_factor(other.m_min_load_factor),
                                         m_max_load_factor(other.m_max_load_factor),
                                         m_grow_on_next_insert(other.m_grow_on_next_insert),
                                         m_try_shrink_on_next_insert(other.m_try_shrink_on_next_insert)
    {
    }

    robin_hash(robin_hash&& other) noexcept(std::is_nothrow_move_constructible<Hash>::value &&
                                            std::is_nothrow_move_constructible<KeyEqual>::value &&
                                            std::is_nothrow_move_constructible<GrowthPolicy>::value &&
                                            std::is_nothrow_move_constructible<buckets_container_type>::value)
                                          : Hash(std::move(static_cast<Hash&>(other))),
                                            KeyEqual(std::move(static_cast<KeyEqual&>(other))),
                                            GrowthPolicy(std::move(static_cast<GrowthPolicy&>(other))),
                                            m_buckets_data(std::move(other.m_buckets_data)),
                                            m_buckets(m_buckets_data.empty()?static_empty_bucket_ptr():m_buckets_data.data()),
                                            m_bucket_count(other.m_bucket_count),
                                            m_nb_elements(other.m_nb_elements),
                                            m_load_threshold(other.m_load_threshold),
                                            m_min_load_factor(other.m_min_load_factor),
                                            m_max_load_factor(other.m_max_load_factor),
                                            m_grow_on_next_insert(other.m_grow_on_next_insert),
                                            m_try_shrink_on_next_insert(other.m_try_shrink_on_next_insert)
    {
        other.clear_and_shrink();
    }

    robin_hash& operator=(const robin_hash& other) {
        if(&other != this) {
            Hash::operator=(other);
            KeyEqual::operator=(other);
            GrowthPolicy::operator=(other);

            m_buckets_data = other.m_buckets_data;
            m_buckets = m_buckets_data.empty()?static_empty_bucket_ptr():
                                               m_buckets_data.data();
            m_bucket_count = other.m_bucket_count;
            m_nb_elements = other.m_nb_elements;

            m_load_threshold = other.m_load_threshold;
            m_min_load_factor = other.m_min_load_factor;
            m_max_load_factor = other.m_max_load_factor;

            m_grow_on_next_insert = other.m_grow_on_next_insert;
            m_try_shrink_on_next_insert = other.m_try_shrink_on_next_insert;
        }

        return *this;
    }

    robin_hash& operator=(robin_hash&& other) {
        other.swap(*this);
        other.clear();

        return *this;
    }

    allocator_type get_allocator() const {
        return m_buckets_data.get_allocator();
    }

    /*
     * Iterators
     */
    iterator begin() noexcept {
        std::size_t i = 0;
        while(i < m_bucket_count && m_buckets[i].empty()) {
            i++;
        }

        return iterator(m_buckets + i);
    }

    const_iterator begin() const noexcept {
        return cbegin();
    }

    const_iterator cbegin() const noexcept {
        std::size_t i = 0;
        while(i < m_bucket_count && m_buckets[i].empty()) {
            i++;
        }

        return const_iterator(m_buckets + i);
    }

    iterator end() noexcept {
        return iterator(m_buckets + m_bucket_count);
    }

    const_iterator end() const noexcept {
        return cend();
    }

    const_iterator cend() const noexcept {
        return const_iterator(m_buckets + m_bucket_count);
    }

    /*
     * Capacity
     */
    bool empty() const noexcept {
        return m_nb_elements == 0;
    }

    size_type size() const noexcept {
        return m_nb_elements;
    }

    size_type max_size() const noexcept {
        return m_buckets_data.max_size();
    }

    /*
     * Modifiers
     */
    void clear() noexcept {
        if(m_min_load_factor > 0.0f) {
            clear_and_shrink();
        }
        else {
            for(auto& bucket: m_buckets_data) {
                bucket.clear();
            }

            m_nb_elements = 0;
            m_grow_on_next_insert = false;
        }
    }

    template<typename P>
    std::pair<iterator, bool> insert(P&& value) {
        return insert_impl(KeySelect()(value), std::forward<P>(value));
    }

    template<typename P>
    iterator insert_hint(const_iterator hint, P&& value) {
        if(hint != cend() && compare_keys(KeySelect()(*hint), KeySelect()(value))) {
            return mutable_iterator(hint);
        }

        return insert(std::forward<P>(value)).first;
    }

    template<class InputIt>
    void insert(InputIt first, InputIt last) {
        if(std::is_base_of<std::forward_iterator_tag,
                           typename std::iterator_traits<InputIt>::iterator_category>::value)
        {
            const auto nb_elements_insert = std::distance(first, last);
            const size_type nb_free_buckets = m_load_threshold - size();
            tsl_rh_assert(m_load_threshold >= size());

            if(nb_elements_insert > 0 && nb_free_buckets < size_type(nb_elements_insert)) {
                reserve(size() + size_type(nb_elements_insert));
            }
        }

        for(; first != last; ++first) {
            insert(*first);
        }
    }

    template<class K, class M>
    std::pair<iterator, bool> insert_or_assign(K&& key, M&& obj) {
        auto it = try_emplace(std::forward<K>(key), std::forward<M>(obj));
        if(!it.second) {
            it.first.value() = std::forward<M>(obj);
        }

        return it;
    }

    template<class K, class M>
    iterator insert_or_assign(const_iterator hint, K&& key, M&& obj) {
        if(hint != cend() && compare_keys(KeySelect()(*hint), key)) {
            auto it = mutable_iterator(hint);
            it.value() = std::forward<M>(obj);

            return it;
        }

        return insert_or_assign(std::forward<K>(key), std::forward<M>(obj)).first;
    }

    template<class... Args>
    std::pair<iterator, bool> emplace(Args&&... args) {
        return insert(value_type(std::forward<Args>(args)...));
    }

    template<class... Args>
    iterator emplace_hint(const_iterator hint, Args&&... args) {
        return insert_hint(hint, value_type(std::forward<Args>(args)...));
    }

    template<class K, class... Args>
    std::pair<iterator, bool> try_emplace(K&& key, Args&&... args) {
        return insert_impl(key, std::piecewise_construct,
                                std::forward_as_tuple(std::forward<K>(key)),
                                std::forward_as_tuple(std::forward<Args>(args)...));
    }

    template<class K, class... Args>
    iterator try_emplace_hint(const_iterator hint, K&& key, Args&&... args) {
        if(hint != cend() && compare_keys(KeySelect()(*hint), key)) {
            return mutable_iterator(hint);
        }

        return try_emplace(std::forward<K>(key), std::forward<Args>(args)...).first;
    }

    /**
     * Here to avoid `template<class K> size_type erase(const K& key)` being used when
     * we use an `iterator` instead of a `const_iterator`.
     */
    iterator erase(iterator pos) {
        erase_from_bucket(pos);

        /**
         * Erase bucket used a backward shift after clearing the bucket.
         * Check if there is a new value in the bucket, if not get the next non-empty.
         */
        if(pos.m_bucket->empty()) {
            ++pos;
        }

        m_try_shrink_on_next_insert = true;

        return pos;
    }

    iterator erase(const_iterator pos) {
        return erase(mutable_iterator(pos));
    }

    iterator erase(const_iterator first, const_iterator last) {
        if(first == last) {
            return mutable_iterator(first);
        }

        auto first_mutable = mutable_iterator(first);
        auto last_mutable = mutable_iterator(last);
        for(auto it = first_mutable.m_bucket; it != last_mutable.m_bucket; ++it) {
            if(!it->empty()) {
                it->clear();
                m_nb_elements--;
            }
        }

        if(last_mutable == end()) {
            m_try_shrink_on_next_insert = true;
            return end();
        }

        /*
         * Backward shift on the values which come after the deleted values.
         * We try to move the values closer to their ideal bucket.
         */
        std::size_t icloser_bucket = static_cast<std::size_t>(first_mutable.m_bucket - m_buckets);
        std::size_t ito_move_closer_value = static_cast<std::size_t>(last_mutable.m_bucket - m_buckets);
        tsl_rh_assert(ito_move_closer_value > icloser_bucket);

        const std::size_t ireturn_bucket = ito_move_closer_value -
                                           std::min(ito_move_closer_value - icloser_bucket,
                                                    std::size_t(m_buckets[ito_move_closer_value].dist_from_ideal_bucket()));

        while(ito_move_closer_value < m_bucket_count && m_buckets[ito_move_closer_value].dist_from_ideal_bucket() > 0) {
            icloser_bucket = ito_move_closer_value -
                             std::min(ito_move_closer_value - icloser_bucket,
                                      std::size_t(m_buckets[ito_move_closer_value].dist_from_ideal_bucket()));

            tsl_rh_assert(m_buckets[icloser_bucket].empty());
            const distance_type new_distance = distance_type(m_buckets[ito_move_closer_value].dist_from_ideal_bucket() -
                                                             (ito_move_closer_value - icloser_bucket));
            m_buckets[icloser_bucket].set_value_of_empty_bucket(new_distance,
                                                                m_buckets[ito_move_closer_value].truncated_hash(),
                                                                std::move(m_buckets[ito_move_closer_value].value()));
            m_buckets[ito_move_closer_value].clear();

            ++icloser_bucket;
            ++ito_move_closer_value;
        }

        m_try_shrink_on_next_insert = true;

        return iterator(m_buckets + ireturn_bucket);
    }

    template<class K>
    size_type erase(const K& key) {
        return erase(key, hash_key(key));
    }

    template<class K>
    size_type erase(const K& key, std::size_t hash) {
        auto it = find(key, hash);
        if(it != end()) {
            erase_from_bucket(it);
            m_try_shrink_on_next_insert = true;

            return 1;
        }
        else {
            return 0;
        }
    }

    void swap(robin_hash& other) {
        using std::swap;

        swap(static_cast<Hash&>(*this), static_cast<Hash&>(other));
        swap(static_cast<KeyEqual&>(*this), static_cast<KeyEqual&>(other));
        swap(static_cast<GrowthPolicy&>(*this), static_cast<GrowthPolicy&>(other));
        swap(m_buckets_data, other.m_buckets_data);
        swap(m_buckets, other.m_buckets);
        swap(m_bucket_count, other.m_bucket_count);
        swap(m_nb_elements, other.m_nb_elements);
        swap(m_load_threshold, other.m_load_threshold);
        swap(m_min_load_factor, other.m_min_load_factor);
        swap(m_max_load_factor, other.m_max_load_factor);
        swap(m_grow_on_next_insert, other.m_grow_on_next_insert);
        swap(m_try_shrink_on_next_insert, other.m_try_shrink_on_next_insert);
    }

    /*
     * Lookup
     */
    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    typename U::value_type& at(const K& key) {
        return at(key, hash_key(key));
    }

    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    typename U::value_type& at(const K& key, std::size_t hash) {
        return const_cast<typename U::value_type&>(static_cast<const robin_hash*>(this)->at(key, hash));
    }

    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    const typename U::value_type& at(const K& key) const {
        return at(key, hash_key(key));
    }

    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    const typename U::value_type& at(const K& key, std::size_t hash) const {
        auto it = find(key, hash);
        if(it != cend()) {
            return it.value();
        }
        else {
            TSL_RH_THROW_OR_TERMINATE(std::out_of_range, "Couldn't find key.");
        }
    }

    template<class K, class U = ValueSelect, typename std::enable_if<has_mapped_type<U>::value>::type* = nullptr>
    typename U::value_type& operator[](K&& key) {
        return try_emplace(std::forward<K>(key)).first.value();
    }

    template<class K>
    size_type count(const K& key) const {
        return count(key, hash_key(key));
    }

    template<class K>
    size_type count(const K& key, std::size_t hash) const {
        if(find(key, hash) != cend()) {
            return 1;
        }
        else {
            return 0;
        }
    }

    template<class K>
    iterator find(const K& key) {
        return find_impl(key, hash_key(key));
    }

    template<class K>
    iterator find(const K& key, std::size_t hash) {
        return find_impl(key, hash);
    }

    template<class K>
    const_iterator find(const K& key) const {
        return find_impl(key, hash_key(key));
    }

    template<class K>
    const_iterator find(const K& key, std::size_t hash) const {
        return find_impl(key, hash);
    }

    template<class K>
    bool contains(const K& key) const {
        return contains(key, hash_key(key));
    }

    template<class K>
    bool contains(const K& key, std::size_t hash) const {
        return count(key, hash) != 0;
    }

    template<class K>
    std::pair<iterator, iterator> equal_range(const K& key) {
        return equal_range(key, hash_key(key));
    }

    template<class K>
    std::pair<iterator, iterator> equal_range(const K& key, std::size_t hash) {
        iterator it = find(key, hash);
        return std::make_pair(it, (it == end())?it:std::next(it));
    }

    template<class K>
    std::pair<const_iterator, const_iterator> equal_range(const K& key) const {
        return equal_range(key, hash_key(key));
    }

    template<class K>
    std::pair<const_iterator, const_iterator> equal_range(const K& key, std::size_t hash) const {
        const_iterator it = find(key, hash);
        return std::make_pair(it, (it == cend())?it:std::next(it));
    }

    /*
     * Bucket interface
     */
    size_type bucket_count() const {
        return m_bucket_count;
    }

    size_type max_bucket_count() const {
        return std::min(GrowthPolicy::max_bucket_count(), m_buckets_data.max_size());
    }

    /*
     * Hash policy
     */
    float load_factor() const {
        if(bucket_count() == 0) {
            return 0;
        }

        return float(m_nb_elements)/float(bucket_count());
    }

    float min_load_factor() const {
        return m_min_load_factor;
    }

    float max_load_factor() const {
        return m_max_load_factor;
    }

    void min_load_factor(float ml) {
        m_min_load_factor = clamp(ml, float(MINIMUM_MIN_LOAD_FACTOR),
                                      float(MAXIMUM_MIN_LOAD_FACTOR));
    }

    void max_load_factor(float ml) {
        m_max_load_factor = clamp(ml, float(MINIMUM_MAX_LOAD_FACTOR),
                                      float(MAXIMUM_MAX_LOAD_FACTOR));
        m_load_threshold = size_type(float(bucket_count())*m_max_load_factor);
    }

    void rehash(size_type count) {
        count = std::max(count, size_type(std::ceil(float(size())/max_load_factor())));
        rehash_impl(count);
    }

    void reserve(size_type count) {
        rehash(size_type(std::ceil(float(count)/max_load_factor())));
    }

    /*
     * Observers
     */
    hasher hash_function() const {
        return static_cast<const Hash&>(*this);
    }

    key_equal key_eq() const {
        return static_cast<const KeyEqual&>(*this);
    }

    /*
     * Other
     */
    iterator mutable_iterator(const_iterator pos) {
        return iterator(const_cast<bucket_entry*>(pos.m_bucket));
    }

private:
    template<class K>
    std::size_t hash_key(const K& key) const {
        return Hash::operator()(key);
    }

    template<class K1, class K2>
    bool compare_keys(const K1& key1, const K2& key2) const {
        return KeyEqual::operator()(key1, key2);
    }

    std::size_t bucket_for_hash(std::size_t hash) const {
        const std::size_t bucket = GrowthPolicy::bucket_for_hash(hash);
        tsl_rh_assert(bucket < m_bucket_count || (bucket == 0 && m_bucket_count == 0));

        return bucket;
    }

    template<class U = GrowthPolicy, typename std::enable_if<is_power_of_two_policy<U>::value>::type* = nullptr>
    std::size_t next_bucket(std::size_t index) const noexcept {
        tsl_rh_assert(index < bucket_count());

        return (index + 1) & this->m_mask;
    }

    template<class U = GrowthPolicy, typename std::enable_if<!is_power_of_two_policy<U>::value>::type* = nullptr>
    std::size_t next_bucket(std::size_t index) const noexcept {
        tsl_rh_assert(index < bucket_count());

        index++;
        return (index != bucket_count())?index:0;
    }

    template<class K>
    iterator find_impl(const K& key, std::size_t hash) {
        return mutable_iterator(static_cast<const robin_hash*>(this)->find(key, hash));
    }

    template<class K>
    const_iterator find_impl(const K& key, std::size_t hash) const {
        std::size_t ibucket = bucket_for_hash(hash);
        distance_type dist_from_ideal_bucket = 0;

        while(dist_from_ideal_bucket <= m_buckets[ibucket].dist_from_ideal_bucket()) {
            if(TSL_RH_LIKELY((!USE_STORED_HASH_ON_LOOKUP || m_buckets[ibucket].bucket_hash_equal(hash)) &&
               compare_keys(KeySelect()(m_buckets[ibucket].value()), key)))
            {
                return const_iterator(m_buckets + ibucket);
            }

            ibucket = next_bucket(ibucket);
            dist_from_ideal_bucket++;
        }

        return cend();
    }

    void erase_from_bucket(iterator pos) {
        pos.m_bucket->clear();
        m_nb_elements--;

        /**
         * Backward shift, swap the empty bucket, previous_ibucket, with the values on its right, ibucket,
         * until we cross another empty bucket or if the other bucket has a distance_from_ideal_bucket == 0.
         *
         * We try to move the values closer to their ideal bucket.
         */
        std::size_t previous_ibucket = static_cast<std::size_t>(pos.m_bucket - m_buckets);
        std::size_t ibucket = next_bucket(previous_ibucket);

        while(m_buckets[ibucket].dist_from_ideal_bucket() > 0) {
            tsl_rh_assert(m_buckets[previous_ibucket].empty());

            const distance_type new_distance = distance_type(m_buckets[ibucket].dist_from_ideal_bucket() - 1);
            m_buckets[previous_ibucket].set_value_of_empty_bucket(new_distance, m_buckets[ibucket].truncated_hash(),
                                                                  std::move(m_buckets[ibucket].value()));
            m_buckets[ibucket].clear();

            previous_ibucket = ibucket;
            ibucket = next_bucket(ibucket);
        }
    }

    template<class K, class... Args>
    std::pair<iterator, bool> insert_impl(const K& key, Args&&... value_type_args) {
        const std::size_t hash = hash_key(key);

        std::size_t ibucket = bucket_for_hash(hash);
        distance_type dist_from_ideal_bucket = 0;

        while(dist_from_ideal_bucket <= m_buckets[ibucket].dist_from_ideal_bucket()) {
            if((!USE_STORED_HASH_ON_LOOKUP || m_buckets[ibucket].bucket_hash_equal(hash)) &&
               compare_keys(KeySelect()(m_buckets[ibucket].value()), key))
            {
                return std::make_pair(iterator(m_buckets + ibucket), false);
            }

            ibucket = next_bucket(ibucket);
            dist_from_ideal_bucket++;
        }

        if(rehash_on_extreme_load()) {
            ibucket = bucket_for_hash(hash);
            dist_from_ideal_bucket = 0;

            while(dist_from_ideal_bucket <= m_buckets[ibucket].dist_from_ideal_bucket()) {
                ibucket = next_bucket(ibucket);
                dist_from_ideal_bucket++;
            }
        }

        if(m_buckets[ibucket].empty()) {
            m_buckets[ibucket].set_value_of_empty_bucket(dist_from_ideal_bucket, bucket_entry::truncate_hash(hash),
                                                         std::forward<Args>(value_type_args)...);
        }
        else {
            insert_value(ibucket, dist_from_ideal_bucket, bucket_entry::truncate_hash(hash),
                         std::forward<Args>(value_type_args)...);
        }

        m_nb_elements++;
        /*
         * The value will be inserted in ibucket in any case, either because it was
         * empty or by stealing the bucket (robin hood).
         */
        return std::make_pair(iterator(m_buckets + ibucket), true);
    }

    template<class... Args>
    void insert_value(std::size_t ibucket, distance_type dist_from_ideal_bucket,
                      truncated_hash_type hash, Args&&... value_type_args)
    {
        value_type value(std::forward<Args>(value_type_args)...);
        insert_value_impl(ibucket, dist_from_ideal_bucket, hash, value);
    }

    void insert_value(std::size_t ibucket, distance_type dist_from_ideal_bucket,
                      truncated_hash_type hash, value_type&& value)
    {
        insert_value_impl(ibucket, dist_from_ideal_bucket, hash, value);
    }

    /*
     * We don't use `value_type&& value` as last argument due to a bug in MSVC when `value_type` is a pointer,
     * The compiler is not able to see the difference between `std::string*` and `std::string*&&` resulting in
     * a compilation error.
     *
     * The `value` will be in a moved state at the end of the function.
     */
    void insert_value_impl(std::size_t ibucket, distance_type dist_from_ideal_bucket,
                           truncated_hash_type hash, value_type& value)
    {
        m_buckets[ibucket].swap_with_value_in_bucket(dist_from_ideal_bucket, hash, value);
        ibucket = next_bucket(ibucket);
        dist_from_ideal_bucket++;

        while(!m_buckets[ibucket].empty()) {
            if(dist_from_ideal_bucket > m_buckets[ibucket].dist_from_ideal_bucket()) {
                if(dist_from_ideal_bucket >= bucket_entry::DIST_FROM_IDEAL_BUCKET_LIMIT) {
                    /**
                     * The number of probes is really high, rehash the map on the next insert.
                     * Difficult to do now as rehash may throw an exception.
                     */
                    m_grow_on_next_insert = true;
                }

                m_buckets[ibucket].swap_with_value_in_bucket(dist_from_ideal_bucket, hash, value);
            }

            ibucket = next_bucket(ibucket);
            dist_from_ideal_bucket++;
        }

        m_buckets[ibucket].set_value_of_empty_bucket(dist_from_ideal_bucket, hash, std::move(value));
    }

    void rehash_impl(size_type count) {
        robin_hash new_table(count, static_cast<Hash&>(*this), static_cast<KeyEqual&>(*this),
                             get_allocator(), m_min_load_factor, m_max_load_factor);

        const bool use_stored_hash = USE_STORED_HASH_ON_REHASH(new_table.bucket_count());
        for(auto& bucket: m_buckets_data) {
            if(bucket.empty()) {
                continue;
            }

            const std::size_t hash = use_stored_hash?bucket.truncated_hash():
                                                     new_table.hash_key(KeySelect()(bucket.value()));

            new_table.insert_value_on_rehash(new_table.bucket_for_hash(hash), 0,
                                             bucket_entry::truncate_hash(hash), std::move(bucket.value()));
        }

        new_table.m_nb_elements = m_nb_elements;
        new_table.swap(*this);
    }

    void clear_and_shrink() noexcept {
        GrowthPolicy::clear();
        m_buckets_data.clear();
        m_buckets = static_empty_bucket_ptr();
        m_bucket_count = 0;
        m_nb_elements = 0;
        m_load_threshold = 0;
        m_grow_on_next_insert = false;
        m_try_shrink_on_next_insert = false;
    }

    void insert_value_on_rehash(std::size_t ibucket, distance_type dist_from_ideal_bucket,
                                truncated_hash_type hash, value_type&& value)
    {
        while(true) {
            if(dist_from_ideal_bucket > m_buckets[ibucket].dist_from_ideal_bucket()) {
                if(m_buckets[ibucket].empty()) {
                    m_buckets[ibucket].set_value_of_empty_bucket(dist_from_ideal_bucket, hash, std::move(value));
                    return;
                }
                else {
                    m_buckets[ibucket].swap_with_value_in_bucket(dist_from_ideal_bucket, hash, value);
                }
            }

            dist_from_ideal_bucket++;
            ibucket = next_bucket(ibucket);
        }
    }

    /**
     * Grow the table if m_grow_on_next_insert is true or we reached the max_load_factor.
     * Shrink the table if m_try_shrink_on_next_insert is true (an erase occurred) and
     * we're below the min_load_factor.
     *
     * Return true if the table has been rehashed.
     */
    bool rehash_on_extreme_load() {
        if(m_grow_on_next_insert || size() >= m_load_threshold) {
            rehash_impl(GrowthPolicy::next_bucket_count());
            m_grow_on_next_insert = false;

            return true;
        }

        if(m_try_shrink_on_next_insert) {
            m_try_shrink_on_next_insert = false;
            if(m_min_load_factor != 0.0f && load_factor() < m_min_load_factor) {
                reserve(size() + 1);

                return true;
            }
        }

        return false;
    }

public:
    static const size_type DEFAULT_INIT_BUCKETS_SIZE = 0;

    static constexpr float DEFAULT_MAX_LOAD_FACTOR = 0.5f;
    static constexpr float MINIMUM_MAX_LOAD_FACTOR = 0.2f;
    static constexpr float MAXIMUM_MAX_LOAD_FACTOR = 0.95f;

    static constexpr float DEFAULT_MIN_LOAD_FACTOR = 0.0f;
    static constexpr float MINIMUM_MIN_LOAD_FACTOR = 0.0f;
    static constexpr float MAXIMUM_MIN_LOAD_FACTOR = 0.15f;

    static_assert(MINIMUM_MAX_LOAD_FACTOR < MAXIMUM_MAX_LOAD_FACTOR,
                  "MINIMUM_MAX_LOAD_FACTOR should be < MAXIMUM_MAX_LOAD_FACTOR");
    static_assert(MINIMUM_MIN_LOAD_FACTOR < MAXIMUM_MIN_LOAD_FACTOR,
                  "MINIMUM_MIN_LOAD_FACTOR should be < MAXIMUM_MIN_LOAD_FACTOR");
    static_assert(MAXIMUM_MIN_LOAD_FACTOR < MINIMUM_MAX_LOAD_FACTOR,
                  "MAXIMUM_MIN_LOAD_FACTOR should be < MINIMUM_MAX_LOAD_FACTOR");

private:
    /**
     * Return an always valid pointer to an static empty bucket_entry with last_bucket() == true.
     */
    bucket_entry* static_empty_bucket_ptr() noexcept {
        static bucket_entry empty_bucket(true);
        return &empty_bucket;
    }

private:
    buckets_container_type m_buckets_data;

    /**
     * Points to m_buckets_data.data() if !m_buckets_data.empty() otherwise points to static_empty_bucket_ptr.
     * This variable is useful to avoid the cost of checking if m_buckets_data is empty when trying
     * to find an element.
     *
     * TODO Remove m_buckets_data and only use a pointer instead of a pointer+vector to save some space in the robin_hash object.
     * Manage the Allocator manually.
     */
    bucket_entry* m_buckets;

    /**
     * Used a lot in find, avoid the call to m_buckets_data.size() which is a bit slower.
     */
    size_type m_bucket_count;

    size_type m_nb_elements;

    size_type m_load_threshold;

    float m_min_load_factor;
    float m_max_load_factor;

    bool m_grow_on_next_insert;

    /**
     * We can't shrink down the map on erase operations as the erase methods need to return the next iterator.
     * Shrinking the map would invalidate all the iterators and we could not return the next iterator in a meaningful way,
     * On erase, we thus just indicate on erase that we should try to shrink the hash table on the next insert
     * if we go below the min_load_factor.
     */
    bool m_try_shrink_on_next_insert;
};

}

}

#endif

class stateOptions {
  public:
    bool do_fej = true;
    bool do_calib_camera_pose = false;
    bool do_calib_camera_intrinsics = false;
    bool do_calib_camera_timeoffset = false;
    bool do_calib_imu_intrinsics = false;
    bool do_calib_imu_g_sensitivity = false;
    ImuModel imu_model = ImuModel::KALIBR;
    int max_clone_size = 11;
    int max_slam_features = 25;
    int max_slam_in_update = 1000;
    int max_msckf_in_update = 1000;
    double voxel_size = 0.5;
    int max_num_points_in_voxel = 10;
    double min_distance_points = 0.15;
    bool use_huber = true;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    double init_window_time = 1.0;
    double init_imu_thresh = 1.0;
    double init_max_disparity = 1.0;
    int init_max_features = 50;
    bool init_dyn_use = false;
    bool init_dyn_mle_opt_calib = false;
    int init_dyn_mle_max_iter = 20;
    int init_dyn_mle_max_threads = 20;
    double init_dyn_mle_max_time = 5.0;
    int init_dyn_num_pose = 5;
    double init_dyn_min_deg = 45.0;
    double init_dyn_inflation_orientation = 10.0;
    double init_dyn_inflation_velocity = 10.0;
    double init_dyn_inflation_bias_gyro = 100.0;
    double init_dyn_inflation_bias_accel = 100.0;
    double init_dyn_min_rec_cond = 1e-15;
    Eigen::Vector3d init_dyn_bias_g = Eigen::Vector3d::Zero();
    Eigen::Vector3d init_dyn_bias_a = Eigen::Vector3d::Zero();
    double sigma_w = 1.6968e-04;
    double sigma_wb = 1.9393e-05;
    double sigma_a = 2.0000e-3;
    double sigma_ab = 3.0000e-03;
    double sigma_pix = 1;
    double gravity_mag = 9.81;
    bool downsample_cameras = false;
    double calib_camimu_dt = 0.0;
    std::unordered_map<size_t, std::shared_ptr<cameraBase>> camera_intrinsics;
    std::map<size_t, Eigen::VectorXd> camera_extrinsics;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    bool refine_features = true;
    int max_runs = 5;
    double init_lamda = 1e-3;
    double max_lamda = 1e10;
    double min_dx = 1e-6;
    double min_dcost = 1e-6;
    double lam_mult = 10;
    double min_dist = 0.10;
    double max_dist = 60;
    double max_baseline = 40;
    double max_cond_number = 10000;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    double sigma_w = 1.6968e-04;
    double sigma_w_2 = pow(1.6968e-04, 2);
    double sigma_wb = 1.9393e-05;
    double sigma_wb_2 = pow(1.9393e-05, 2);
    double sigma_a = 2.0000e-3;
    double sigma_a_2 = pow(2.0000e-3, 2);
    double sigma_ab = 3.0000e-03;
    double sigma_ab_2 = pow(3.0000e-03, 2);
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    double chi2_multipler = 5;
    double sigma_pix = 1;
    double sigma_pix_sq = 1;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
  public:
    stateOptions state_options;
    inertialInitializerOptions init_options;
    double dt_slam_delay = 2.0;
    noiseManager imu_noises;
    updaterOptions msckf_options;
    updaterOptions slam_options;
    double gravity_mag = 9.81;
    Eigen::Matrix<double, 6, 1> vec_dw;
    Eigen::Matrix<double, 6, 1> vec_da;
    Eigen::Matrix<double, 9, 1> vec_tg;
    Eigen::Matrix<double, 4, 1> q_imu_acc;
    Eigen::Matrix<double, 4, 1> q_imu_gyr;
    double calib_camimu_dt = 0.0;
    std::unordered_map<size_t, std::shared_ptr<cameraBase>> camera_intrinsics;
    std::map<size_t, Eigen::VectorXd> camera_extrinsics;
    bool use_mask = false;
    std::map<size_t, cv::Mat> masks;
    bool downsample_cameras = false;
    int num_pts = 150;
    int fast_threshold = 20;
    int patch_size_x = 5;
    int patch_size_y = 5;
    int min_px_dist = 10;
    HistogramMethod histogram_method = HistogramMethod::HISTOGRAM;
    double track_frequency = 20.0;
    featureInitializerOptions featinit_options;
    double voxel_size = 0.5;
    int max_num_points_in_voxel = 10;
    double min_distance_points = 0.15;
    int nb_voxels_visited = 1;
    bool use_all_points = false;
    bool use_keyframe = true;
    void recordParameters() {
	state_options.recordParameters();

	init_options.recordParameters();

	std::string str_temp;

	std::ofstream foutC(std::string(output_path + "/parameter_list.txt"), std::ios::app);

	foutC << std::fixed << "dt_slam_delay: " << dt_slam_delay << std::endl;

	imu_noises.recordParameters();

	msckf_options.recordParameters();

	slam_options.recordParameters();

	foutC << std::fixed << "gravity_mag: " << gravity_mag << std::endl;

	foutC << std::fixed << "vec_dw: " << vec_dw.transpose() << std::endl;

	foutC << std::fixed << "vec_da: " << vec_da.transpose() << std::endl;

	foutC << std::fixed << "vec_tg: " << vec_tg.transpose() << std::endl;

	foutC << std::fixed << "q_imu_acc: " << q_imu_acc.w() << " " << q_imu_acc.x() << " " << q_imu_acc.y() << " " << q_imu_acc.z() << std::endl;

	foutC << std::fixed << "q_imu_gyr: " << q_imu_gyr.w() << " " << q_imu_gyr.x() << " " << q_imu_gyr.y() << " " << q_imu_gyr.z() << std::endl;

	foutC << std::fixed << "calib_camimu_dt: " << calib_camimu_dt << std::endl;

	for (int i = 0; i < 2; i++)
	{
		if (i == 0) foutC << "left camera parameters: " << std::endl;
		else foutC << "right camera parameters: " << std::endl;

		foutC << std::fixed << "camera intrinic: " << camera_intrinsics.at(i)->getValue().transpose() << std::endl;
		foutC << std::fixed << "camera_k_OPENCV: "
			  << camera_intrinsics.at(i)->getK()(0, 0) << " " << camera_intrinsics.at(i)->getK()(0, 1) << " " << camera_intrinsics.at(i)->getK()(0, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(1, 0) << " " << camera_intrinsics.at(i)->getK()(1, 1) << " " << camera_intrinsics.at(i)->getK()(1, 2) << std::endl
			  << camera_intrinsics.at(i)->getK()(2, 0) << " " << camera_intrinsics.at(i)->getK()(2, 1) << " " << camera_intrinsics.at(i)->getK()(2, 2) << std::endl;

		foutC << std::fixed << "camera_d_OPENCV: "
			  << camera_intrinsics.at(i)->getD()(0) << " " << camera_intrinsics.at(i)->getD()(1) << " "
			  << camera_intrinsics.at(i)->getD()(2) << " " << camera_intrinsics.at(i)->getD()(3) << std::endl;

		foutC << "image width: " << camera_intrinsics.at(i)->w() << std::endl;
		foutC << "image height: " << camera_intrinsics.at(i)->h() << std::endl;

		foutC << std::fixed << "camera_extrinsics: " << camera_extrinsics.at(i).transpose() << std::endl;
	}

	str_temp = use_mask ? "true" : "false";
	foutC << "use_mask: " << str_temp << std::endl;

	str_temp = downsample_cameras ? "true" : "false";
	foutC << "downsample_cameras: " << str_temp << std::endl;

	foutC << "num_pts: " << num_pts << std::endl;

	foutC << "fast_threshold: " << fast_threshold << std::endl;

	foutC << "patch_size_x: " << patch_size_x << std::endl;

	foutC << "patch_size_y: " << patch_size_y << std::endl;

	foutC << "min_px_dist: " << min_px_dist << std::endl;

	switch (histogram_method)
	{
	case 0:
		str_temp = "none";
		break;
	case 1:
		str_temp = "histogram";
		break;
	case 2:
		str_temp = "clahe";
		break;
	default:
		break;
	}
	foutC << "histogram_method: " << str_temp << std::endl;

	foutC << std::fixed << "track_frequency: " << track_frequency << std::endl;

	featinit_options.recordParameters();

	foutC.close();
}
};

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_GROWTH_POLICY_H
#define TSL_ROBIN_GROWTH_POLICY_H

#include <algorithm>
#include <array>
#include <climits>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <iterator>
#include <limits>
#include <ratio>
#include <stdexcept>

#ifdef TSL_DEBUG
#    define tsl_rh_assert(expr) assert(expr)
#else
#    define tsl_rh_assert(expr) (static_cast<void>(0))
#endif

/**
 * If exceptions are enabled, throw the exception passed in parameter, otherwise call std::terminate.
 */
#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || (defined (_MSC_VER) && defined (_CPPUNWIND))) && !defined(TSL_NO_EXCEPTIONS)
#    define TSL_RH_THROW_OR_TERMINATE(ex, msg) throw ex(msg)
#else
#    define TSL_RH_NO_EXCEPTIONS
#    ifdef NDEBUG
#        define TSL_RH_THROW_OR_TERMINATE(ex, msg) std::terminate()
#    else
#        include <iostream>
#        define TSL_RH_THROW_OR_TERMINATE(ex, msg) do { std::cerr << msg << std::endl; std::terminate(); } while(0)
#    endif
#endif

#if defined(__GNUC__) || defined(__clang__)
#    define TSL_RH_LIKELY(exp) (__builtin_expect(!!(exp), true))
#else
#    define TSL_RH_LIKELY(exp) (exp)
#endif

namespace tsl {
namespace rh {

/**
 * Grow the hash table by a factor of GrowthFactor keeping the bucket count to a power of two. It allows
 * the table to use a mask operation instead of a modulo operation to map a hash to a bucket.
 *
 * GrowthFactor must be a power of two >= 2.
 */
template<std::size_t GrowthFactor>
class power_of_two_growth_policy {
public:
    /**
     * Called on the hash table creation and on rehash. The number of buckets for the table is passed in parameter.
     * This number is a minimum, the policy may update this value with a higher value if needed (but not lower).
     *
     * If 0 is given, min_bucket_count_in_out must still be 0 after the policy creation and
     * bucket_for_hash must always return 0 in this case.
     */
    explicit power_of_two_growth_policy(std::size_t& min_bucket_count_in_out) {
        if(min_bucket_count_in_out > max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(min_bucket_count_in_out > 0) {
            min_bucket_count_in_out = round_up_to_power_of_two(min_bucket_count_in_out);
            m_mask = min_bucket_count_in_out - 1;
        }
        else {
            m_mask = 0;
        }
    }

    /**
     * Return the bucket [0, bucket_count()) to which the hash belongs.
     * If bucket_count() is 0, it must always return 0.
     */
    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return hash & m_mask;
    }

    /**
     * Return the number of buckets that should be used on next growth.
     */
    std::size_t next_bucket_count() const {
        if((m_mask + 1) > max_bucket_count() / GrowthFactor) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        return (m_mask + 1) * GrowthFactor;
    }

    /**
     * Return the maximum number of buckets supported by the policy.
     */
    std::size_t max_bucket_count() const {
        // Largest power of two.
        return (std::numeric_limits<std::size_t>::max() / 2) + 1;
    }

    /**
     * Reset the growth policy as if it was created with a bucket count of 0.
     * After a clear, the policy must always return 0 when bucket_for_hash is called.
     */
    void clear() noexcept {
        m_mask = 0;
    }

private:
    static std::size_t round_up_to_power_of_two(std::size_t value) {
        if(is_power_of_two(value)) {
            return value;
        }

        if(value == 0) {
            return 1;
        }

        --value;
        for(std::size_t i = 1; i < sizeof(std::size_t) * CHAR_BIT; i *= 2) {
            value |= value >> i;
        }

        return value + 1;
    }

    static constexpr bool is_power_of_two(std::size_t value) {
        return value != 0 && (value & (value - 1)) == 0;
    }

protected:
    static_assert(is_power_of_two(GrowthFactor) && GrowthFactor >= 2, "GrowthFactor must be a power of two >= 2.");

    std::size_t m_mask;
};

/**
 * Grow the hash table by GrowthFactor::num / GrowthFactor::den and use a modulo to map a hash
 * to a bucket. Slower but it can be useful if you want a slower growth.
 */
template<class GrowthFactor = std::ratio<3, 2>>
class mod_growth_policy {
public:
    explicit mod_growth_policy(std::size_t& min_bucket_count_in_out) {
        if(min_bucket_count_in_out > max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(min_bucket_count_in_out > 0) {
            m_mod = min_bucket_count_in_out;
        }
        else {
            m_mod = 1;
        }
    }

    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return hash % m_mod;
    }

    std::size_t next_bucket_count() const {
        if(m_mod == max_bucket_count()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        const double next_bucket_count = std::ceil(double(m_mod) * REHASH_SIZE_MULTIPLICATION_FACTOR);
        if(!std::isnormal(next_bucket_count)) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        if(next_bucket_count > double(max_bucket_count())) {
            return max_bucket_count();
        }
        else {
            return std::size_t(next_bucket_count);
        }
    }

    std::size_t max_bucket_count() const {
        return MAX_BUCKET_COUNT;
    }

    void clear() noexcept {
        m_mod = 1;
    }

private:
    static constexpr double REHASH_SIZE_MULTIPLICATION_FACTOR = 1.0 * GrowthFactor::num / GrowthFactor::den;
    static const std::size_t MAX_BUCKET_COUNT =
            std::size_t(double(
                    std::numeric_limits<std::size_t>::max() / REHASH_SIZE_MULTIPLICATION_FACTOR
            ));

    static_assert(REHASH_SIZE_MULTIPLICATION_FACTOR >= 1.1, "Growth factor should be >= 1.1.");

    std::size_t m_mod;
};

namespace detail {

#if SIZE_MAX >= ULLONG_MAX
#define TSL_RH_NB_PRIMES 51
#elif SIZE_MAX >= ULONG_MAX
#define TSL_RH_NB_PRIMES 40
#else
#define TSL_RH_NB_PRIMES 23
#endif

static constexpr const std::array<std::size_t, TSL_RH_NB_PRIMES> PRIMES = {{
    1u, 5u, 17u, 29u, 37u, 53u, 67u, 79u, 97u, 131u, 193u, 257u, 389u, 521u, 769u, 1031u,
    1543u, 2053u, 3079u, 6151u, 12289u, 24593u, 49157u,
#if SIZE_MAX >= ULONG_MAX
    98317ul, 196613ul, 393241ul, 786433ul, 1572869ul, 3145739ul, 6291469ul, 12582917ul,
    25165843ul, 50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul, 1610612741ul,
    3221225473ul, 4294967291ul,
#endif
#if SIZE_MAX >= ULLONG_MAX
    6442450939ull, 12884901893ull, 25769803751ull, 51539607551ull, 103079215111ull, 206158430209ull,
    412316860441ull, 824633720831ull, 1649267441651ull, 3298534883309ull, 6597069766657ull,
#endif
}};

template<unsigned int IPrime>
static constexpr std::size_t mod(std::size_t hash) { return hash % PRIMES[IPrime]; }

// MOD_PRIME[iprime](hash) returns hash % PRIMES[iprime]. This table allows for faster modulo as the
// compiler can optimize the modulo code better with a constant known at the compilation.
static constexpr const std::array<std::size_t(*)(std::size_t), TSL_RH_NB_PRIMES> MOD_PRIME = {{
    &mod<0>, &mod<1>, &mod<2>, &mod<3>, &mod<4>, &mod<5>, &mod<6>, &mod<7>, &mod<8>, &mod<9>, &mod<10>,
    &mod<11>, &mod<12>, &mod<13>, &mod<14>, &mod<15>, &mod<16>, &mod<17>, &mod<18>, &mod<19>, &mod<20>,
    &mod<21>, &mod<22>,
#if SIZE_MAX >= ULONG_MAX
    &mod<23>, &mod<24>, &mod<25>, &mod<26>, &mod<27>, &mod<28>, &mod<29>, &mod<30>, &mod<31>, &mod<32>,
    &mod<33>, &mod<34>, &mod<35>, &mod<36>, &mod<37> , &mod<38>, &mod<39>,
#endif
#if SIZE_MAX >= ULLONG_MAX
    &mod<40>, &mod<41>, &mod<42>, &mod<43>, &mod<44>, &mod<45>, &mod<46>, &mod<47>, &mod<48>, &mod<49>,
    &mod<50>,
#endif
}};

}

/**
 * Grow the hash table by using prime numbers as bucket count. Slower than tsl::rh::power_of_two_growth_policy in
 * general but will probably distribute the values around better in the buckets with a poor hash function.
 *
 * To allow the compiler to optimize the modulo operation, a lookup table is used with constant primes numbers.
 *
 * With a switch the code would look like:
 * \code
 * switch(iprime) { // iprime is the current prime of the hash table
 *     case 0: hash % 5ul;
 *             break;
 *     case 1: hash % 17ul;
 *             break;
 *     case 2: hash % 29ul;
 *             break;
 *     ...
 * }
 * \endcode
 *
 * Due to the constant variable in the modulo the compiler is able to optimize the operation
 * by a series of multiplications, substractions and shifts.
 *
 * The 'hash % 5' could become something like 'hash - (hash * 0xCCCCCCCD) >> 34) * 5' in a 64 bits environment.
 */
class prime_growth_policy {
public:
    explicit prime_growth_policy(std::size_t& min_bucket_count_in_out) {
        auto it_prime = std::lower_bound(detail::PRIMES.begin(),
                                         detail::PRIMES.end(), min_bucket_count_in_out);
        if(it_prime == detail::PRIMES.end()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        m_iprime = static_cast<unsigned int>(std::distance(detail::PRIMES.begin(), it_prime));
        if(min_bucket_count_in_out > 0) {
            min_bucket_count_in_out = *it_prime;
        }
        else {
            min_bucket_count_in_out = 0;
        }
    }

    std::size_t bucket_for_hash(std::size_t hash) const noexcept {
        return detail::MOD_PRIME[m_iprime](hash);
    }

    std::size_t next_bucket_count() const {
        if(m_iprime + 1 >= detail::PRIMES.size()) {
            TSL_RH_THROW_OR_TERMINATE(std::length_error, "The hash table exceeds its maximum size.");
        }

        return detail::PRIMES[m_iprime + 1];
    }

    std::size_t max_bucket_count() const {
        return detail::PRIMES.back();
    }

    void clear() noexcept {
        m_iprime = 0;
    }

private:
    unsigned int m_iprime;

    static_assert(std::numeric_limits<decltype(m_iprime)>::max() >= detail::PRIMES.size(),
                  "The type of m_iprime is not big enough.");
};

}
}

#endif

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_MAP_H
#define TSL_ROBIN_MAP_H

#include <cstddef>
#include <functional>
#include <initializer_list>
#include <memory>
#include <type_traits>
#include <utility>
#include "robin_hash.h"

namespace tsl {

/**
 * Implementation of a hash map using open-addressing and the robin hood hashing algorithm with backward shift deletion.
 *
 * For operations modifying the hash map (insert, erase, rehash, ...), the strong exception guarantee
 * is only guaranteed when the expression `std::is_nothrow_swappable<std::pair<Key, T>>::value &&
 * std::is_nothrow_move_constructible<std::pair<Key, T>>::value` is true, otherwise if an exception
 * is thrown during the swap or the move, the hash map may end up in a undefined state. Per the standard
 * a `Key` or `T` with a noexcept copy constructor and no move constructor also satisfies the
 * `std::is_nothrow_move_constructible<std::pair<Key, T>>::value` criterion (and will thus guarantee the
 * strong exception for the map).
 *
 * When `StoreHash` is true, 32 bits of the hash are stored alongside the values. It can improve
 * the performance during lookups if the `KeyEqual` function takes time (if it engenders a cache-miss for example)
 * as we then compare the stored hashes before comparing the keys. When `tsl::rh::power_of_two_growth_policy` is used
 * as `GrowthPolicy`, it may also speed-up the rehash process as we can avoid to recalculate the hash.
 * When it is detected that storing the hash will not incur any memory penalty due to alignment (i.e.
 * `sizeof(tsl::detail_robin_hash::bucket_entry<ValueType, true>) ==
 * sizeof(tsl::detail_robin_hash::bucket_entry<ValueType, false>)`) and `tsl::rh::power_of_two_growth_policy` is
 * used, the hash will be stored even if `StoreHash` is false so that we can speed-up the rehash (but it will
 * not be used on lookups unless `StoreHash` is true).
 *
 * `GrowthPolicy` defines how the map grows and consequently how a hash value is mapped to a bucket.
 * By default the map uses `tsl::rh::power_of_two_growth_policy`. This policy keeps the number of buckets
 * to a power of two and uses a mask to map the hash to a bucket instead of the slow modulo.
 * Other growth policies are available and you may define your own growth policy,
 * check `tsl::rh::power_of_two_growth_policy` for the interface.
 *
 * `std::pair<Key, T>` must be swappable.
 *
 * `Key` and `T` must be copy and/or move constructible.
 *
 * If the destructor of `Key` or `T` throws an exception, the behaviour of the class is undefined.
 *
 * Iterators invalidation:
 *  - clear, operator=, reserve, rehash: always invalidate the iterators.
 *  - insert, emplace, emplace_hint, operator[]: if there is an effective insert, invalidate the iterators.
 *  - erase: always invalidate the iterators.
 */
template<class Key,
         class T,
         class Hash = std::hash<Key>,
         class KeyEqual = std::equal_to<Key>,
         class Allocator = std::allocator<std::pair<Key, T>>,
         bool StoreHash = false,
         class GrowthPolicy = tsl::rh::power_of_two_growth_policy<2>>
class robin_map {
private:
    template<typename U>
    using has_is_transparent = tsl::detail_robin_hash::has_is_transparent<U>;

    class KeySelect {
    public:
        using key_type = Key;

        const key_type& operator()(const std::pair<Key, T>& key_value) const noexcept {
            return key_value.first;
        }

        key_type& operator()(std::pair<Key, T>& key_value) noexcept {
            return key_value.first;
        }
    };

    class ValueSelect {
    public:
        using value_type = T;

        const value_type& operator()(const std::pair<Key, T>& key_value) const noexcept {
            return key_value.second;
        }

        value_type& operator()(std::pair<Key, T>& key_value) noexcept {
            return key_value.second;
        }
    };

    using ht = detail_robin_hash::robin_hash<std::pair<Key, T>, KeySelect, ValueSelect,
                                             Hash, KeyEqual, Allocator, StoreHash, GrowthPolicy>;

public:
    using key_type = typename ht::key_type;
    using mapped_type = T;
    using value_type = typename ht::value_type;
    using size_type = typename ht::size_type;
    using difference_type = typename ht::difference_type;
    using hasher = typename ht::hasher;
    using key_equal = typename ht::key_equal;
    using allocator_type = typename ht::allocator_type;
    using reference = typename ht::reference;
    using const_reference = typename ht::const_reference;
    using pointer = typename ht::pointer;
    using const_pointer = typename ht::const_pointer;
    using iterator = typename ht::iterator;
    using const_iterator = typename ht::const_iterator;

public:
    /*
     * Constructors
     */
    robin_map(): robin_map(ht::DEFAULT_INIT_BUCKETS_SIZE) {
    }

    explicit robin_map(size_type bucket_count,
                       const Hash& hash = Hash(),
                       const KeyEqual& equal = KeyEqual(),
                       const Allocator& alloc = Allocator()):
                m_ht(bucket_count, hash, equal, alloc)
    {
    }

    robin_map(size_type bucket_count,
              const Allocator& alloc): robin_map(bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    robin_map(size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc): robin_map(bucket_count, hash, KeyEqual(), alloc)
    {
    }

    explicit robin_map(const Allocator& alloc): robin_map(ht::DEFAULT_INIT_BUCKETS_SIZE, alloc) {
    }

    template<class InputIt>
    robin_map(InputIt first, InputIt last,
              size_type bucket_count = ht::DEFAULT_INIT_BUCKETS_SIZE,
              const Hash& hash = Hash(),
              const KeyEqual& equal = KeyEqual(),
              const Allocator& alloc = Allocator()): robin_map(bucket_count, hash, equal, alloc)
    {
        insert(first, last);
    }

    template<class InputIt>
    robin_map(InputIt first, InputIt last,
              size_type bucket_count,
              const Allocator& alloc): robin_map(first, last, bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    template<class InputIt>
    robin_map(InputIt first, InputIt last,
              size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc): robin_map(first, last, bucket_count, hash, KeyEqual(), alloc)
    {
    }

    robin_map(std::initializer_list<value_type> init,
              size_type bucket_count = ht::DEFAULT_INIT_BUCKETS_SIZE,
              const Hash& hash = Hash(),
              const KeyEqual& equal = KeyEqual(),
              const Allocator& alloc = Allocator()):
          robin_map(init.begin(), init.end(), bucket_count, hash, equal, alloc)
    {
    }

    robin_map(std::initializer_list<value_type> init,
              size_type bucket_count,
              const Allocator& alloc):
          robin_map(init.begin(), init.end(), bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    robin_map(std::initializer_list<value_type> init,
              size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc):
          robin_map(init.begin(), init.end(), bucket_count, hash, KeyEqual(), alloc)
    {
    }

    robin_map& operator=(std::initializer_list<value_type> ilist) {
        m_ht.clear();

        m_ht.reserve(ilist.size());
        m_ht.insert(ilist.begin(), ilist.end());

        return *this;
    }

    allocator_type get_allocator() const { return m_ht.get_allocator(); }

    /*
     * Iterators
     */
    iterator begin() noexcept { return m_ht.begin(); }
    const_iterator begin() const noexcept { return m_ht.begin(); }
    const_iterator cbegin() const noexcept { return m_ht.cbegin(); }

    iterator end() noexcept { return m_ht.end(); }
    const_iterator end() const noexcept { return m_ht.end(); }
    const_iterator cend() const noexcept { return m_ht.cend(); }

    /*
     * Capacity
     */
    bool empty() const noexcept { return m_ht.empty(); }
    size_type size() const noexcept { return m_ht.size(); }
    size_type max_size() const noexcept { return m_ht.max_size(); }

    /*
     * Modifiers
     */
    void clear() noexcept { m_ht.clear(); }

    std::pair<iterator, bool> insert(const value_type& value) {
        return m_ht.insert(value);
    }

    template<class P, typename std::enable_if<std::is_constructible<value_type, P&&>::value>::type* = nullptr>
    std::pair<iterator, bool> insert(P&& value) {
        return m_ht.emplace(std::forward<P>(value));
    }

    std::pair<iterator, bool> insert(value_type&& value) {
        return m_ht.insert(std::move(value));
    }

    iterator insert(const_iterator hint, const value_type& value) {
        return m_ht.insert_hint(hint, value);
    }

    template<class P, typename std::enable_if<std::is_constructible<value_type, P&&>::value>::type* = nullptr>
    iterator insert(const_iterator hint, P&& value) {
        return m_ht.emplace_hint(hint, std::forward<P>(value));
    }

    iterator insert(const_iterator hint, value_type&& value) {
        return m_ht.insert_hint(hint, std::move(value));
    }

    template<class InputIt>
    void insert(InputIt first, InputIt last) {
        m_ht.insert(first, last);
    }

    void insert(std::initializer_list<value_type> ilist) {
        m_ht.insert(ilist.begin(), ilist.end());
    }

    template<class M>
    std::pair<iterator, bool> insert_or_assign(const key_type& k, M&& obj) {
        return m_ht.insert_or_assign(k, std::forward<M>(obj));
    }

    template<class M>
    std::pair<iterator, bool> insert_or_assign(key_type&& k, M&& obj) {
        return m_ht.insert_or_assign(std::move(k), std::forward<M>(obj));
    }

    template<class M>
    iterator insert_or_assign(const_iterator hint, const key_type& k, M&& obj) {
        return m_ht.insert_or_assign(hint, k, std::forward<M>(obj));
    }

    template<class M>
    iterator insert_or_assign(const_iterator hint, key_type&& k, M&& obj) {
        return m_ht.insert_or_assign(hint, std::move(k), std::forward<M>(obj));
    }

    /**
     * Due to the way elements are stored, emplace will need to move or copy the key-value once.
     * The method is equivalent to insert(value_type(std::forward<Args>(args)...));
     *
     * Mainly here for compatibility with the std::unordered_map interface.
     */
    template<class... Args>
    std::pair<iterator, bool> emplace(Args&&... args) {
        return m_ht.emplace(std::forward<Args>(args)...);
    }

    /**
     * Due to the way elements are stored, emplace_hint will need to move or copy the key-value once.
     * The method is equivalent to insert(hint, value_type(std::forward<Args>(args)...));
     *
     * Mainly here for compatibility with the std::unordered_map interface.
     */
    template<class... Args>
    iterator emplace_hint(const_iterator hint, Args&&... args) {
        return m_ht.emplace_hint(hint, std::forward<Args>(args)...);
    }

    template<class... Args>
    std::pair<iterator, bool> try_emplace(const key_type& k, Args&&... args) {
        return m_ht.try_emplace(k, std::forward<Args>(args)...);
    }

    template<class... Args>
    std::pair<iterator, bool> try_emplace(key_type&& k, Args&&... args) {
        return m_ht.try_emplace(std::move(k), std::forward<Args>(args)...);
    }

    template<class... Args>
    iterator try_emplace(const_iterator hint, const key_type& k, Args&&... args) {
        return m_ht.try_emplace_hint(hint, k, std::forward<Args>(args)...);
    }

    template<class... Args>
    iterator try_emplace(const_iterator hint, key_type&& k, Args&&... args) {
        return m_ht.try_emplace_hint(hint, std::move(k), std::forward<Args>(args)...);
    }

    iterator erase(iterator pos) { return m_ht.erase(pos); }
    iterator erase(const_iterator pos) { return m_ht.erase(pos); }
    iterator erase(const_iterator first, const_iterator last) { return m_ht.erase(first, last); }
    size_type erase(const key_type& key) { return m_ht.erase(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup to the value if you already have the hash.
     */
    size_type erase(const key_type& key, std::size_t precalculated_hash) {
        return m_ht.erase(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type erase(const K& key) { return m_ht.erase(key); }

    /**
     * @copydoc erase(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup to the value if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type erase(const K& key, std::size_t precalculated_hash) {
        return m_ht.erase(key, precalculated_hash);
    }

    void swap(robin_map& other) { other.m_ht.swap(m_ht); }

    /*
     * Lookup
     */
    T& at(const Key& key) { return m_ht.at(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    T& at(const Key& key, std::size_t precalculated_hash) { return m_ht.at(key, precalculated_hash); }

    const T& at(const Key& key) const { return m_ht.at(key); }

    /**
     * @copydoc at(const Key& key, std::size_t precalculated_hash)
     */
    const T& at(const Key& key, std::size_t precalculated_hash) const { return m_ht.at(key, precalculated_hash); }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    T& at(const K& key) { return m_ht.at(key); }

    /**
     * @copydoc at(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    T& at(const K& key, std::size_t precalculated_hash) { return m_ht.at(key, precalculated_hash); }

    /**
     * @copydoc at(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const T& at(const K& key) const { return m_ht.at(key); }

    /**
     * @copydoc at(const K& key, std::size_t precalculated_hash)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const T& at(const K& key, std::size_t precalculated_hash) const { return m_ht.at(key, precalculated_hash); }

    T& operator[](const Key& key) { return m_ht[key]; }
    T& operator[](Key&& key) { return m_ht[std::move(key)]; }

    size_type count(const Key& key) const { return m_ht.count(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    size_type count(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.count(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type count(const K& key) const { return m_ht.count(key); }

    /**
     * @copydoc count(const K& key) const
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type count(const K& key, std::size_t precalculated_hash) const { return m_ht.count(key, precalculated_hash); }

    iterator find(const Key& key) { return m_ht.find(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    iterator find(const Key& key, std::size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }

    const_iterator find(const Key& key) const { return m_ht.find(key); }

    /**
     * @copydoc find(const Key& key, std::size_t precalculated_hash)
     */
    const_iterator find(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.find(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    iterator find(const K& key) { return m_ht.find(key); }

    /**
     * @copydoc find(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    iterator find(const K& key, std::size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }

    /**
     * @copydoc find(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const_iterator find(const K& key) const { return m_ht.find(key); }

    /**
     * @copydoc find(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const_iterator find(const K& key, std::size_t precalculated_hash) const {
        return m_ht.find(key, precalculated_hash);
    }

    bool contains(const Key& key) const { return m_ht.contains(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    bool contains(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.contains(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    bool contains(const K& key) const { return m_ht.contains(key); }

    /**
     * @copydoc contains(const K& key) const
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    bool contains(const K& key, std::size_t precalculated_hash) const {
        return m_ht.contains(key, precalculated_hash);
    }

    std::pair<iterator, iterator> equal_range(const Key& key) { return m_ht.equal_range(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    std::pair<iterator, iterator> equal_range(const Key& key, std::size_t precalculated_hash) {
        return m_ht.equal_range(key, precalculated_hash);
    }

    std::pair<const_iterator, const_iterator> equal_range(const Key& key) const { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const Key& key, std::size_t precalculated_hash)
     */
    std::pair<const_iterator, const_iterator> equal_range(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<iterator, iterator> equal_range(const K& key) { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<iterator, iterator> equal_range(const K& key, std::size_t precalculated_hash) {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /**
     * @copydoc equal_range(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<const_iterator, const_iterator> equal_range(const K& key) const { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const K& key, std::size_t precalculated_hash)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<const_iterator, const_iterator> equal_range(const K& key, std::size_t precalculated_hash) const {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /*
     * Bucket interface
     */
    size_type bucket_count() const { return m_ht.bucket_count(); }
    size_type max_bucket_count() const { return m_ht.max_bucket_count(); }

    /*
     *  Hash policy
     */
    float load_factor() const { return m_ht.load_factor(); }

    float min_load_factor() const { return m_ht.min_load_factor(); }
    float max_load_factor() const { return m_ht.max_load_factor(); }

    /**
     * Set the `min_load_factor` to `ml`. When the `load_factor` of the map goes
     * below `min_load_factor` after some erase operations, the map will be
     * shrunk when an insertion occurs. The erase method itself never shrinks
     * the map.
     *
     * The default value of `min_load_factor` is 0.0f, the map never shrinks by default.
     */
    void min_load_factor(float ml) { m_ht.min_load_factor(ml); }
    void max_load_factor(float ml) { m_ht.max_load_factor(ml); }

    void rehash(size_type count) { m_ht.rehash(count); }
    void reserve(size_type count) { m_ht.reserve(count); }

    /*
     * Observers
     */
    hasher hash_function() const { return m_ht.hash_function(); }
    key_equal key_eq() const { return m_ht.key_eq(); }

    /*
     * Other
     */

    /**
     * Convert a const_iterator to an iterator.
     */
    iterator mutable_iterator(const_iterator pos) {
        return m_ht.mutable_iterator(pos);
    }

    friend bool operator==(const robin_map& lhs, const robin_map& rhs) {
        if(lhs.size() != rhs.size()) {
            return false;
        }

        for(const auto& element_lhs: lhs) {
            const auto it_element_rhs = rhs.find(element_lhs.first);
            if(it_element_rhs == rhs.cend() || element_lhs.second != it_element_rhs->second) {
                return false;
            }
        }

        return true;
    }

    friend bool operator!=(const robin_map& lhs, const robin_map& rhs) {
        return !operator==(lhs, rhs);
    }

    friend void swap(robin_map& lhs, robin_map& rhs) {
        lhs.swap(rhs);
    }

private:
    ht m_ht;
};

/**
 * Same as `tsl::robin_map<Key, T, Hash, KeyEqual, Allocator, StoreHash, tsl::rh::prime_growth_policy>`.
 */
template<class Key,
         class T,
         class Hash = std::hash<Key>,
         class KeyEqual = std::equal_to<Key>,
         class Allocator = std::allocator<std::pair<Key, T>>,
         bool StoreHash = false>
using robin_pg_map = robin_map<Key, T, Hash, KeyEqual, Allocator, StoreHash, tsl::rh::prime_growth_policy>;

} // end namespace tsl

#endif

/**
 * MIT License
 *
 * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#ifndef TSL_ROBIN_SET_H
#define TSL_ROBIN_SET_H

#include <cstddef>
#include <functional>
#include <initializer_list>
#include <memory>
#include <type_traits>
#include <utility>
#include "robin_hash.h"

namespace tsl {

/**
 * Implementation of a hash set using open-addressing and the robin hood hashing algorithm with backward shift deletion.
 *
 * For operations modifying the hash set (insert, erase, rehash, ...), the strong exception guarantee
 * is only guaranteed when the expression `std::is_nothrow_swappable<Key>::value &&
 * std::is_nothrow_move_constructible<Key>::value` is true, otherwise if an exception
 * is thrown during the swap or the move, the hash set may end up in a undefined state. Per the standard
 * a `Key` with a noexcept copy constructor and no move constructor also satisfies the
 * `std::is_nothrow_move_constructible<Key>::value` criterion (and will thus guarantee the
 * strong exception for the set).
 *
 * When `StoreHash` is true, 32 bits of the hash are stored alongside the values. It can improve
 * the performance during lookups if the `KeyEqual` function takes time (or engenders a cache-miss for example)
 * as we then compare the stored hashes before comparing the keys. When `tsl::rh::power_of_two_growth_policy` is used
 * as `GrowthPolicy`, it may also speed-up the rehash process as we can avoid to recalculate the hash.
 * When it is detected that storing the hash will not incur any memory penalty due to alignment (i.e.
 * `sizeof(tsl::detail_robin_hash::bucket_entry<ValueType, true>) ==
 * sizeof(tsl::detail_robin_hash::bucket_entry<ValueType, false>)`) and `tsl::rh::power_of_two_growth_policy` is
 * used, the hash will be stored even if `StoreHash` is false so that we can speed-up the rehash (but it will
 * not be used on lookups unless `StoreHash` is true).
 *
 * `GrowthPolicy` defines how the set grows and consequently how a hash value is mapped to a bucket.
 * By default the set uses `tsl::rh::power_of_two_growth_policy`. This policy keeps the number of buckets
 * to a power of two and uses a mask to set the hash to a bucket instead of the slow modulo.
 * Other growth policies are available and you may define your own growth policy,
 * check `tsl::rh::power_of_two_growth_policy` for the interface.
 *
 * `Key` must be swappable.
 *
 * `Key` must be copy and/or move constructible.
 *
 * If the destructor of `Key` throws an exception, the behaviour of the class is undefined.
 *
 * Iterators invalidation:
 *  - clear, operator=, reserve, rehash: always invalidate the iterators.
 *  - insert, emplace, emplace_hint, operator[]: if there is an effective insert, invalidate the iterators.
 *  - erase: always invalidate the iterators.
 */
template<class Key,
         class Hash = std::hash<Key>,
         class KeyEqual = std::equal_to<Key>,
         class Allocator = std::allocator<Key>,
         bool StoreHash = false,
         class GrowthPolicy = tsl::rh::power_of_two_growth_policy<2>>
class robin_set {
private:
    template<typename U>
    using has_is_transparent = tsl::detail_robin_hash::has_is_transparent<U>;

    class KeySelect {
    public:
        using key_type = Key;

        const key_type& operator()(const Key& key) const noexcept {
            return key;
        }

        key_type& operator()(Key& key) noexcept {
            return key;
        }
    };

    using ht = detail_robin_hash::robin_hash<Key, KeySelect, void,
                                             Hash, KeyEqual, Allocator, StoreHash, GrowthPolicy>;

public:
    using key_type = typename ht::key_type;
    using value_type = typename ht::value_type;
    using size_type = typename ht::size_type;
    using difference_type = typename ht::difference_type;
    using hasher = typename ht::hasher;
    using key_equal = typename ht::key_equal;
    using allocator_type = typename ht::allocator_type;
    using reference = typename ht::reference;
    using const_reference = typename ht::const_reference;
    using pointer = typename ht::pointer;
    using const_pointer = typename ht::const_pointer;
    using iterator = typename ht::iterator;
    using const_iterator = typename ht::const_iterator;

    /*
     * Constructors
     */
    robin_set(): robin_set(ht::DEFAULT_INIT_BUCKETS_SIZE) {
    }

    explicit robin_set(size_type bucket_count,
                       const Hash& hash = Hash(),
                       const KeyEqual& equal = KeyEqual(),
                       const Allocator& alloc = Allocator()):
                    m_ht(bucket_count, hash, equal, alloc)
    {
    }

    robin_set(size_type bucket_count,
              const Allocator& alloc): robin_set(bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    robin_set(size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc): robin_set(bucket_count, hash, KeyEqual(), alloc)
    {
    }

    explicit robin_set(const Allocator& alloc): robin_set(ht::DEFAULT_INIT_BUCKETS_SIZE, alloc) {
    }

    template<class InputIt>
    robin_set(InputIt first, InputIt last,
              size_type bucket_count = ht::DEFAULT_INIT_BUCKETS_SIZE,
              const Hash& hash = Hash(),
              const KeyEqual& equal = KeyEqual(),
              const Allocator& alloc = Allocator()): robin_set(bucket_count, hash, equal, alloc)
    {
        insert(first, last);
    }

    template<class InputIt>
    robin_set(InputIt first, InputIt last,
              size_type bucket_count,
              const Allocator& alloc): robin_set(first, last, bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    template<class InputIt>
    robin_set(InputIt first, InputIt last,
              size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc): robin_set(first, last, bucket_count, hash, KeyEqual(), alloc)
    {
    }

    robin_set(std::initializer_list<value_type> init,
              size_type bucket_count = ht::DEFAULT_INIT_BUCKETS_SIZE,
              const Hash& hash = Hash(),
              const KeyEqual& equal = KeyEqual(),
              const Allocator& alloc = Allocator()):
          robin_set(init.begin(), init.end(), bucket_count, hash, equal, alloc)
    {
    }

    robin_set(std::initializer_list<value_type> init,
              size_type bucket_count,
              const Allocator& alloc):
          robin_set(init.begin(), init.end(), bucket_count, Hash(), KeyEqual(), alloc)
    {
    }

    robin_set(std::initializer_list<value_type> init,
              size_type bucket_count,
              const Hash& hash,
              const Allocator& alloc):
          robin_set(init.begin(), init.end(), bucket_count, hash, KeyEqual(), alloc)
    {
    }

    robin_set& operator=(std::initializer_list<value_type> ilist) {
        m_ht.clear();

        m_ht.reserve(ilist.size());
        m_ht.insert(ilist.begin(), ilist.end());

        return *this;
    }

    allocator_type get_allocator() const { return m_ht.get_allocator(); }

    /*
     * Iterators
     */
    iterator begin() noexcept { return m_ht.begin(); }
    const_iterator begin() const noexcept { return m_ht.begin(); }
    const_iterator cbegin() const noexcept { return m_ht.cbegin(); }

    iterator end() noexcept { return m_ht.end(); }
    const_iterator end() const noexcept { return m_ht.end(); }
    const_iterator cend() const noexcept { return m_ht.cend(); }

    /*
     * Capacity
     */
    bool empty() const noexcept { return m_ht.empty(); }
    size_type size() const noexcept { return m_ht.size(); }
    size_type max_size() const noexcept { return m_ht.max_size(); }

    /*
     * Modifiers
     */
    void clear() noexcept { m_ht.clear(); }

    std::pair<iterator, bool> insert(const value_type& value) {
        return m_ht.insert(value);
    }

    std::pair<iterator, bool> insert(value_type&& value) {
        return m_ht.insert(std::move(value));
    }

    iterator insert(const_iterator hint, const value_type& value) {
        return m_ht.insert_hint(hint, value);
    }

    iterator insert(const_iterator hint, value_type&& value) {
        return m_ht.insert_hint(hint, std::move(value));
    }

    template<class InputIt>
    void insert(InputIt first, InputIt last) {
        m_ht.insert(first, last);
    }

    void insert(std::initializer_list<value_type> ilist) {
        m_ht.insert(ilist.begin(), ilist.end());
    }

    /**
     * Due to the way elements are stored, emplace will need to move or copy the key-value once.
     * The method is equivalent to insert(value_type(std::forward<Args>(args)...));
     *
     * Mainly here for compatibility with the std::unordered_map interface.
     */
    template<class... Args>
    std::pair<iterator, bool> emplace(Args&&... args) {
        return m_ht.emplace(std::forward<Args>(args)...);
    }

    /**
     * Due to the way elements are stored, emplace_hint will need to move or copy the key-value once.
     * The method is equivalent to insert(hint, value_type(std::forward<Args>(args)...));
     *
     * Mainly here for compatibility with the std::unordered_map interface.
     */
    template<class... Args>
    iterator emplace_hint(const_iterator hint, Args&&... args) {
        return m_ht.emplace_hint(hint, std::forward<Args>(args)...);
    }

    iterator erase(iterator pos) { return m_ht.erase(pos); }
    iterator erase(const_iterator pos) { return m_ht.erase(pos); }
    iterator erase(const_iterator first, const_iterator last) { return m_ht.erase(first, last); }
    size_type erase(const key_type& key) { return m_ht.erase(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup to the value if you already have the hash.
     */
    size_type erase(const key_type& key, std::size_t precalculated_hash) {
        return m_ht.erase(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type erase(const K& key) { return m_ht.erase(key); }

    /**
     * @copydoc erase(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup to the value if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type erase(const K& key, std::size_t precalculated_hash) {
        return m_ht.erase(key, precalculated_hash);
    }

    void swap(robin_set& other) { other.m_ht.swap(m_ht); }

    /*
     * Lookup
     */
    size_type count(const Key& key) const { return m_ht.count(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    size_type count(const Key& key, std::size_t precalculated_hash) const { return m_ht.count(key, precalculated_hash); }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type count(const K& key) const { return m_ht.count(key); }

    /**
     * @copydoc count(const K& key) const
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    size_type count(const K& key, std::size_t precalculated_hash) const { return m_ht.count(key, precalculated_hash); }

    iterator find(const Key& key) { return m_ht.find(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    iterator find(const Key& key, std::size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }

    const_iterator find(const Key& key) const { return m_ht.find(key); }

    /**
     * @copydoc find(const Key& key, std::size_t precalculated_hash)
     */
    const_iterator find(const Key& key, std::size_t precalculated_hash) const { return m_ht.find(key, precalculated_hash); }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    iterator find(const K& key) { return m_ht.find(key); }

    /**
     * @copydoc find(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    iterator find(const K& key, std::size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }

    /**
     * @copydoc find(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const_iterator find(const K& key) const { return m_ht.find(key); }

    /**
     * @copydoc find(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    const_iterator find(const K& key, std::size_t precalculated_hash) const { return m_ht.find(key, precalculated_hash); }

    bool contains(const Key& key) const { return m_ht.contains(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    bool contains(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.contains(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    bool contains(const K& key) const { return m_ht.contains(key); }

    /**
     * @copydoc contains(const K& key) const
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    bool contains(const K& key, std::size_t precalculated_hash) const {
        return m_ht.contains(key, precalculated_hash);
    }

    std::pair<iterator, iterator> equal_range(const Key& key) { return m_ht.equal_range(key); }

    /**
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    std::pair<iterator, iterator> equal_range(const Key& key, std::size_t precalculated_hash) {
        return m_ht.equal_range(key, precalculated_hash);
    }

    std::pair<const_iterator, const_iterator> equal_range(const Key& key) const { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const Key& key, std::size_t precalculated_hash)
     */
    std::pair<const_iterator, const_iterator> equal_range(const Key& key, std::size_t precalculated_hash) const {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /**
     * This overload only participates in the overload resolution if the typedef KeyEqual::is_transparent exists.
     * If so, K must be hashable and comparable to Key.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<iterator, iterator> equal_range(const K& key) { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const K& key)
     *
     * Use the hash value 'precalculated_hash' instead of hashing the key. The hash value should be the same
     * as hash_function()(key). Useful to speed-up the lookup if you already have the hash.
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<iterator, iterator> equal_range(const K& key, std::size_t precalculated_hash) {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /**
     * @copydoc equal_range(const K& key)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<const_iterator, const_iterator> equal_range(const K& key) const { return m_ht.equal_range(key); }

    /**
     * @copydoc equal_range(const K& key, std::size_t precalculated_hash)
     */
    template<class K, class KE = KeyEqual, typename std::enable_if<has_is_transparent<KE>::value>::type* = nullptr>
    std::pair<const_iterator, const_iterator> equal_range(const K& key, std::size_t precalculated_hash) const {
        return m_ht.equal_range(key, precalculated_hash);
    }

    /*
     * Bucket interface
     */
    size_type bucket_count() const { return m_ht.bucket_count(); }
    size_type max_bucket_count() const { return m_ht.max_bucket_count(); }

    /*
     *  Hash policy
     */
    float load_factor() const { return m_ht.load_factor(); }

    float min_load_factor() const { return m_ht.min_load_factor(); }
    float max_load_factor() const { return m_ht.max_load_factor(); }

    /**
     * Set the `min_load_factor` to `ml`. When the `load_factor` of the set goes
     * below `min_load_factor` after some erase operations, the set will be
     * shrunk when an insertion occurs. The erase method itself never shrinks
     * the set.
     *
     * The default value of `min_load_factor` is 0.0f, the set never shrinks by default.
     */
    void min_load_factor(float ml) { m_ht.min_load_factor(ml); }
    void max_load_factor(float ml) { m_ht.max_load_factor(ml); }

    void rehash(size_type count) { m_ht.rehash(count); }
    void reserve(size_type count) { m_ht.reserve(count); }

    /*
     * Observers
     */
    hasher hash_function() const { return m_ht.hash_function(); }
    key_equal key_eq() const { return m_ht.key_eq(); }

    /*
     * Other
     */

    /**
     * Convert a const_iterator to an iterator.
     */
    iterator mutable_iterator(const_iterator pos) {
        return m_ht.mutable_iterator(pos);
    }

    friend bool operator==(const robin_set& lhs, const robin_set& rhs) {
        if(lhs.size() != rhs.size()) {
            return false;
        }

        for(const auto& element_lhs: lhs) {
            const auto it_element_rhs = rhs.find(element_lhs);
            if(it_element_rhs == rhs.cend()) {
                return false;
            }
        }

        return true;
    }

    friend bool operator!=(const robin_set& lhs, const robin_set& rhs) {
        return !operator==(lhs, rhs);
    }

    friend void swap(robin_set& lhs, robin_set& rhs) {
        lhs.swap(rhs);
    }

private:
    ht m_ht;
};

/**
 * Same as `tsl::robin_set<Key, Hash, KeyEqual, Allocator, StoreHash, tsl::rh::prime_growth_policy>`.
 */
template<class Key,
         class Hash = std::hash<Key>,
         class KeyEqual = std::equal_to<Key>,
         class Allocator = std::allocator<Key>,
         bool StoreHash = false>
using robin_pg_set = robin_set<Key, Hash, KeyEqual, Allocator, StoreHash, tsl::rh::prime_growth_policy>;

} // end namespace tsl

#endif

This code segment functions to: Initialize data structures for gradient-based pixel selection by allocating memory for randomized sampling patterns, histogram buffers, and threshold values while setting initial parameters for spatial subdivision and processing efficiency.

```cpp
[TASK_BEGIN]
pixelSelector(int w, int h) {
    current_potential = 3;
    allow_fast = true;

    random_pattern = new unsigned char[w*h];
    for(int i=0;i<w*h;i++) random_pattern[i] = rand()&0xFF;

    grad_hist = new int[50];

    int w32 = (w+31)/32;
    int h32 = (h+31)/32;
    ths = new float[w32*h32];
    ths_smoothed = new float[w32*h32];
    ths_step = w32;

    wG[0] = w;
    wG[1] = w/2;
    wG[2] = w/4;
    hG[0] = h;
    hG[1] = h/2;
    hG[2] = h/4;
}
[TASK_END]
```