1139 First Contact (30 point(s))

本文介绍了一种在社交网络中寻找共同好友的算法,通过构建图数据结构并使用哈希映射来加速查找过程,实现了对同性关系的高效匹配。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

题解

技巧性在于交际网络中,只录入同性关系。

#include<iostream>
#include<cstdio>
#include<map>
#include<algorithm>
#include<vector>
using namespace std;
const int MAXN = 1e4;
struct node {
	int x, y;
	bool operator < (const node& rhs) const {
		return x != rhs.x ? x < rhs.x : y < rhs.y;
	} 
};
vector<int> v[MAXN];
map<int, bool> mp;
int n, m, k;
int main() {
	scanf("%d%d", &n, &m);
	for(int i = 0; i < m; ++i) {
		string a, b;
		cin >> a >> b;
		if(a.length() == b.length()) {
			v[abs(stoi(a))].push_back(abs(stoi(b)));
			v[abs(stoi(b))].push_back(abs(stoi(a)));
		}
		mp[abs(stoi(a)) * 10000 + abs(stoi(b))] = mp[abs(stoi(b)) * 10000 + abs(stoi(a))] = true;
	}
	scanf("%d", &k);
	for(int i = 0; i < k; ++i) {
		int a, b;
		cin >> a >> b;
		vector<node> res;
		for(int j = 0; j < v[abs(a)].size(); ++j) {
			for(int l = 0; l < v[abs(b)].size(); ++l) {
				if(v[abs(a)][j] == abs(b) || v[abs(b)][l] == abs(a)) continue;
				if(mp[v[abs(a)][j] * 10000 + v[abs(b)][l]]) res.push_back(node{v[abs(a)][j], v[abs(b)][l]});
			}
		}
		sort(res.begin(), res.end());
		printf("%d\n", res.size());
		for(int j = 0; j < res.size(); ++j) {
			printf("%04d %04d\n", res[j].x, res[j].y);
		}
	}
	return 0;
}

 

/* This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. Developer: Chunran Zheng <zhengcr@connect.hku.hk> For commercial use, please contact me at <zhengcr@connect.hku.hk> or Prof. Fu Zhang at <fuzhang@hku.hk>. This file is subject to the terms and conditions outlined in the 'LICENSE' file, which is included as part of this source code package. */ #include "IMU_Processing.h" ImuProcess::ImuProcess() : Eye3d(M3D::Identity()), Zero3d(0, 0, 0), b_first_frame(true), imu_need_init(true) { init_iter_num = 1; cov_acc = V3D(0.1, 0.1, 0.1); cov_gyr = V3D(0.1, 0.1, 0.1); cov_bias_gyr = V3D(0.1, 0.1, 0.1); cov_bias_acc = V3D(0.1, 0.1, 0.1); cov_inv_expo = 0.2; mean_acc = V3D(0, 0, -1.0); mean_gyr = V3D(0, 0, 0); angvel_last = Zero3d; acc_s_last = Zero3d; Lid_offset_to_IMU = Zero3d; Lid_rot_to_IMU = Eye3d; last_imu.reset(new sensor_msgs::Imu()); cur_pcl_un_.reset(new PointCloudXYZI()); } ImuProcess::~ImuProcess() {} void ImuProcess::Reset() { ROS_WARN("Reset ImuProcess"); mean_acc = V3D(0, 0, -1.0); mean_gyr = V3D(0, 0, 0); angvel_last = Zero3d; imu_need_init = true; init_iter_num = 1; IMUpose.clear(); last_imu.reset(new sensor_msgs::Imu()); cur_pcl_un_.reset(new PointCloudXYZI()); } void ImuProcess::disable_imu() { cout << "IMU Disabled !!!!!" << endl; imu_en = false; imu_need_init = false; } void ImuProcess::disable_gravity_est() { cout << "Online Gravity Estimation Disabled !!!!!" << endl; gravity_est_en = false; } void ImuProcess::disable_bias_est() { cout << "Bias Estimation Disabled !!!!!" << endl; ba_bg_est_en = false; } void ImuProcess::disable_exposure_est() { cout << "Online Time Offset Estimation Disabled !!!!!" << endl; exposure_estimate_en = false; } void ImuProcess::set_extrinsic(const MD(4, 4) & T) { Lid_offset_to_IMU = T.block<3, 1>(0, 3); Lid_rot_to_IMU = T.block<3, 3>(0, 0); } void ImuProcess::set_extrinsic(const V3D &transl) { Lid_offset_to_IMU = transl; Lid_rot_to_IMU.setIdentity(); } void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) { Lid_offset_to_IMU = transl; Lid_rot_to_IMU = rot; } void ImuProcess::set_gyr_cov_scale(const V3D &scaler) { cov_gyr = scaler; } void ImuProcess::set_acc_cov_scale(const V3D &scaler) { cov_acc = scaler; } void ImuProcess::set_gyr_bias_cov(const V3D &b_g) { cov_bias_gyr = b_g; } void ImuProcess::set_inv_expo_cov(const double &inv_expo) { cov_inv_expo = inv_expo; } void ImuProcess::set_acc_bias_cov(const V3D &b_a) { cov_bias_acc = b_a; } void ImuProcess::set_imu_init_frame_num(const int &num) { MAX_INI_COUNT = num; } void ImuProcess::IMU_init(const MeasureGroup &meas, StatesGroup &state_inout, int &N) { /** 1. initializing the gravity, gyro bias, acc and gyro covariance ** 2. normalize the acceleration measurenments to unit gravity **/ ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); V3D cur_acc, cur_gyr; if (b_first_frame) { Reset(); N = 1; b_first_frame = false; const auto &imu_acc = meas.imu.front()->linear_acceleration; const auto &gyr_acc = meas.imu.front()->angular_velocity; mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; // first_lidar_time = meas.lidar_frame_beg_time; // cout<<"init acc norm: "<<mean_acc.norm()<<endl; } for (const auto &imu : meas.imu) { const auto &imu_acc = imu->linear_acceleration; const auto &gyr_acc = imu->angular_velocity; cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; mean_acc += (cur_acc - mean_acc) / N; mean_gyr += (cur_gyr - mean_gyr) / N; // cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - // mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); cov_gyr // = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - // mean_gyr) * (N - 1.0) / (N * N); // cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl; N++; } IMU_mean_acc_norm = mean_acc.norm(); state_inout.gravity = -mean_acc / mean_acc.norm() * G_m_s2; state_inout.rot_end = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity))); state_inout.bias_g = Zero3d; // mean_gyr; last_imu = meas.imu.back(); } void ImuProcess::Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out) { pcl_out = *(meas.lidar); /*** sort point clouds by offset time ***/ const double &pcl_beg_time = meas.lidar_frame_beg_time; sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); meas.last_lio_update_time = pcl_end_time; const double &pcl_end_offset_time = pcl_out.points.back().curvature / double(1000); MD(DIM_STATE, DIM_STATE) F_x, cov_w; double dt = 0; if (b_first_frame) { dt = 0.1; b_first_frame = false; } else { dt = pcl_beg_time - time_last_scan; } time_last_scan = pcl_beg_time; // for (size_t i = 0; i < pcl_out->points.size(); i++) { // if (dt < pcl_out->points[i].curvature) { // dt = pcl_out->points[i].curvature; // } // } // dt = dt / (double)1000; // std::cout << "dt:" << dt << std::endl; // double dt = pcl_out->points.back().curvature / double(1000); /* covariance propagation */ // M3D acc_avr_skew; M3D Exp_f = Exp(state_inout.bias_g, dt); F_x.setIdentity(); cov_w.setZero(); F_x.block<3, 3>(0, 0) = Exp(state_inout.bias_g, -dt); F_x.block<3, 3>(0, 10) = Eye3d * dt; F_x.block<3, 3>(3, 7) = Eye3d * dt; // F_x.block<3, 3>(6, 0) = - R_imu * acc_avr_skew * dt; // F_x.block<3, 3>(6, 12) = - R_imu * dt; // F_x.block<3, 3>(6, 15) = Eye3d * dt; cov_w.block<3, 3>(10, 10).diagonal() = cov_gyr * dt * dt; // for omega in constant model cov_w.block<3, 3>(7, 7).diagonal() = cov_acc * dt * dt; // for velocity in constant model // cov_w.block<3, 3>(6, 6) = // R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt; // cov_w.block<3, 3>(9, 9).diagonal() = // cov_bias_gyr * dt * dt; // bias gyro covariance // cov_w.block<3, 3>(12, 12).diagonal() = // cov_bias_acc * dt * dt; // bias acc covariance // std::cout << "before propagete:" << state_inout.cov.diagonal().transpose() // << std::endl; state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; // std::cout << "cov_w:" << cov_w.diagonal().transpose() << std::endl; // std::cout << "after propagete:" << state_inout.cov.diagonal().transpose() // << std::endl; state_inout.rot_end = state_inout.rot_end * Exp_f; state_inout.pos_end = state_inout.pos_end + state_inout.vel_end * dt; if (lidar_type != L515) { auto it_pcl = pcl_out.points.end() - 1; double dt_j = 0.0; for(; it_pcl != pcl_out.points.begin(); it_pcl--) { dt_j= pcl_end_offset_time - it_pcl->curvature/double(1000); M3D R_jk(Exp(state_inout.bias_g, - dt_j)); V3D P_j(it_pcl->x, it_pcl->y, it_pcl->z); // Using rotation and translation to un-distort points V3D p_jk; p_jk = - state_inout.rot_end.transpose() * state_inout.vel_end * dt_j; V3D P_compensate = R_jk * P_j + p_jk; /// save Undistorted points and their rotation it_pcl->x = P_compensate(0); it_pcl->y = P_compensate(1); it_pcl->z = P_compensate(2); } } } void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out) { double t0 = omp_get_wtime(); pcl_out.clear(); /*** add the imu of the last frame-tail to the of current frame-head ***/ MeasureGroup &meas = lidar_meas.measures.back(); // cout<<"meas.imu.size: "<<meas.imu.size()<<endl; auto v_imu = meas.imu; v_imu.push_front(last_imu); const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); const double &imu_end_time = v_imu.back()->header.stamp.toSec(); const double prop_beg_time = last_prop_end_time; // printf("[ IMU ] undistort input size: %zu \n", lidar_meas.pcl_proc_cur->points.size()); // printf("[ IMU ] IMU data sequence size: %zu \n", meas.imu.size()); // printf("[ IMU ] lidar_scan_index_now: %d \n", lidar_meas.lidar_scan_index_now); const double prop_end_time = lidar_meas.lio_vio_flg == LIO ? meas.lio_time : meas.vio_time; /*** cut lidar point based on the propagation-start time and required * propagation-end time ***/ // const double pcl_offset_time = (prop_end_time - // lidar_meas.lidar_frame_beg_time) * 1000.; // the offset time w.r.t scan // start time auto pcl_it = lidar_meas.pcl_proc_cur->points.begin() + // lidar_meas.lidar_scan_index_now; auto pcl_it_end = // lidar_meas.lidar->points.end(); printf("[ IMU ] pcl_it->curvature: %lf // pcl_offset_time: %lf \n", pcl_it->curvature, pcl_offset_time); while // (pcl_it != pcl_it_end && pcl_it->curvature <= pcl_offset_time) // { // pcl_wait_proc.push_back(*pcl_it); // pcl_it++; // lidar_meas.lidar_scan_index_now++; // } // cout<<"pcl_out.size(): "<<pcl_out.size()<<endl; // cout<<"pcl_offset_time: "<<pcl_offset_time<<"pcl_it->curvature: // "<<pcl_it->curvature<<endl; // cout<<"lidar_meas.lidar_scan_index_now:"<<lidar_meas.lidar_scan_index_now<<endl; // printf("[ IMU ] last propagation end time: %lf \n", lidar_meas.last_lio_update_time); if (lidar_meas.lio_vio_flg == LIO) { pcl_wait_proc.resize(lidar_meas.pcl_proc_cur->points.size()); pcl_wait_proc = *(lidar_meas.pcl_proc_cur); lidar_meas.lidar_scan_index_now = 0; IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end)); } // printf("[ IMU ] pcl_wait_proc size: %zu \n", pcl_wait_proc.points.size()); // sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); // lidar_meas.debug_show(); // cout<<"UndistortPcl [ IMU ]: Process lidar from "<<prop_beg_time<<" to // "<<prop_end_time<<", " \ // <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to // "<<imu_end_time<<endl; // cout<<"[ IMU ]: point size: "<<lidar_meas.lidar->points.size()<<endl; /*** Initialize IMU pose ***/ // IMUpose.clear(); /*** forward propagation at each imu point ***/ V3D acc_imu(acc_s_last), angvel_avr(angvel_last), acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end); // cout << "[ IMU ] input state: " << state_inout.vel_end.transpose() << " " << state_inout.pos_end.transpose() << endl; M3D R_imu(state_inout.rot_end); MD(DIM_STATE, DIM_STATE) F_x, cov_w; double dt, dt_all = 0.0; double offs_t; // double imu_time; double tau; if (!imu_time_init) { // imu_time = v_imu.front()->header.stamp.toSec() - first_lidar_time; // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); tau = 1.0; imu_time_init = true; } else { tau = state_inout.inv_expo_time; // ROS_ERROR("tau: %.6f !!!!!!", tau); } // state_inout.cov(6, 6) = 0.01; // ROS_ERROR("lidar_meas.lio_vio_flg"); // cout<<"lidar_meas.lio_vio_flg: "<<lidar_meas.lio_vio_flg<<endl; switch (lidar_meas.lio_vio_flg) { case LIO: case VIO: dt = 0; for (int i = 0; i < v_imu.size() - 1; i++) { auto head = v_imu[i]; auto tail = v_imu[i + 1]; if (tail->header.stamp.toSec() < prop_beg_time) continue; angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); // angvel_avr<<tail->angular_velocity.x, tail->angular_velocity.y, // tail->angular_velocity.z; acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); // cout<<"angvel_avr: "<<angvel_avr.transpose()<<endl; // cout<<"acc_avr: "<<acc_avr.transpose()<<endl; // #ifdef DEBUG_PRINT fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; // #endif // imu_time = head->header.stamp.toSec() - first_lidar_time; angvel_avr -= state_inout.bias_g; acc_avr = acc_avr * G_m_s2 / mean_acc.norm() - state_inout.bias_a; if (head->header.stamp.toSec() < prop_beg_time) { // printf("00 \n"); dt = tail->header.stamp.toSec() - last_prop_end_time; offs_t = tail->header.stamp.toSec() - prop_beg_time; } else if (i != v_imu.size() - 2) { // printf("11 \n"); dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); offs_t = tail->header.stamp.toSec() - prop_beg_time; } else { // printf("22 \n"); dt = prop_end_time - head->header.stamp.toSec(); offs_t = prop_end_time - prop_beg_time; } dt_all += dt; // printf("[ LIO Propagation ] dt: %lf \n", dt); /* covariance propagation */ M3D acc_avr_skew; M3D Exp_f = Exp(angvel_avr, dt); acc_avr_skew << SKEW_SYM_MATRX(acc_avr); F_x.setIdentity(); cov_w.setZero(); F_x.block<3, 3>(0, 0) = Exp(angvel_avr, -dt); if (ba_bg_est_en) F_x.block<3, 3>(0, 10) = -Eye3d * dt; // F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt; F_x.block<3, 3>(3, 7) = Eye3d * dt; F_x.block<3, 3>(7, 0) = -R_imu * acc_avr_skew * dt; if (ba_bg_est_en) F_x.block<3, 3>(7, 13) = -R_imu * dt; if (gravity_est_en) F_x.block<3, 3>(7, 16) = Eye3d * dt; // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); // F_x(6,6) = 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) * (-tau*tau); F_x(18,18) = 0.00001; if (exposure_estimate_en) cov_w(6, 6) = cov_inv_expo * dt * dt; cov_w.block<3, 3>(0, 0).diagonal() = cov_gyr * dt * dt; cov_w.block<3, 3>(7, 7) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt; cov_w.block<3, 3>(10, 10).diagonal() = cov_bias_gyr * dt * dt; // bias gyro covariance cov_w.block<3, 3>(13, 13).diagonal() = cov_bias_acc * dt * dt; // bias acc covariance state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; // state_inout.cov.block<18,18>(0,0) = F_x.block<18,18>(0,0) * // state_inout.cov.block<18,18>(0,0) * F_x.block<18,18>(0,0).transpose() + // cov_w.block<18,18>(0,0); // tau = tau + 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) * // (-tau*tau) * dt; // tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75); /* propogation of IMU attitude */ R_imu = R_imu * Exp_f; /* Specific acceleration (global frame) of IMU */ acc_imu = R_imu * acc_avr + state_inout.gravity; /* propogation of IMU */ pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt; /* velocity of IMU */ vel_imu = vel_imu + acc_imu * dt; /* save the poses at each IMU measurements */ angvel_last = angvel_avr; acc_s_last = acc_imu; // cout<<setw(20)<<"offset_t: "<<offs_t<<"tail->header.stamp.toSec(): // "<<tail->header.stamp.toSec()<<endl; printf("[ LIO Propagation ] // offs_t: %lf \n", offs_t); IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu)); } // unbiased_gyr = V3D(IMUpose.back().gyr[0], IMUpose.back().gyr[1], IMUpose.back().gyr[2]); // cout<<"prop end - start: "<<prop_end_time - prop_beg_time<<" dt_all: "<<dt_all<<endl; lidar_meas.last_lio_update_time = prop_end_time; // dt = prop_end_time - imu_end_time; // printf("[ LIO Propagation ] dt: %lf \n", dt); break; } state_inout.vel_end = vel_imu; state_inout.rot_end = R_imu; state_inout.pos_end = pos_imu; state_inout.inv_expo_time = tau; /*** calculated the pos and attitude prediction at the frame-end ***/ // if (imu_end_time>prop_beg_time) // { // double note = prop_end_time > imu_end_time ? 1.0 : -1.0; // dt = note * (prop_end_time - imu_end_time); // state_inout.vel_end = vel_imu + note * acc_imu * dt; // state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt); // state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 * // acc_imu * dt * dt; // } // else // { // double note = prop_end_time > prop_beg_time ? 1.0 : -1.0; // dt = note * (prop_end_time - prop_beg_time); // state_inout.vel_end = vel_imu + note * acc_imu * dt; // state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt); // state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 * // acc_imu * dt * dt; // } // cout<<"[ Propagation ] output state: "<<state_inout.vel_end.transpose() << // state_inout.pos_end.transpose()<<endl; last_imu = v_imu.back(); last_prop_end_time = prop_end_time; double t1 = omp_get_wtime(); // auto pos_liD_e = state_inout.pos_end + state_inout.rot_end * // Lid_offset_to_IMU; auto R_liD_e = state_inout.rot_end * Lidar_R_to_IMU; // cout<<"[ IMU ]: vel "<<state_inout.vel_end.transpose()<<" pos // "<<state_inout.pos_end.transpose()<<" // ba"<<state_inout.bias_a.transpose()<<" bg // "<<state_inout.bias_g.transpose()<<endl; cout<<"propagated cov: // "<<state_inout.cov.diagonal().transpose()<<endl; // cout<<"UndistortPcl Time:"; // for (auto it = IMUpose.begin(); it != IMUpose.end(); ++it) { // cout<<it->offset_time<<" "; // } // cout<<endl<<"UndistortPcl size:"<<IMUpose.size()<<endl; // cout<<"Undistorted pcl_out.size: "<<pcl_out.size() // <<"lidar_meas.size: "<<lidar_meas.lidar->points.size()<<endl; if (pcl_wait_proc.points.size() < 1) return; /*** undistort each lidar point (backward propagation), ONLY working for LIO * update ***/ if (lidar_meas.lio_vio_flg == LIO) { auto it_pcl = pcl_wait_proc.points.end() - 1; M3D extR_Ri(Lid_rot_to_IMU.transpose() * state_inout.rot_end.transpose()); V3D exrR_extT(Lid_rot_to_IMU.transpose() * Lid_offset_to_IMU); for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) { auto head = it_kp - 1; auto tail = it_kp; R_imu << MAT_FROM_ARRAY(head->rot); acc_imu << VEC_FROM_ARRAY(head->acc); // cout<<"head imu acc: "<<acc_imu.transpose()<<endl; vel_imu << VEC_FROM_ARRAY(head->vel); pos_imu << VEC_FROM_ARRAY(head->pos); angvel_avr << VEC_FROM_ARRAY(head->gyr); // printf("head->offset_time: %lf \n", head->offset_time); // printf("it_pcl->curvature: %lf pt dt: %lf \n", it_pcl->curvature, // it_pcl->curvature / double(1000) - head->offset_time); for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) { dt = it_pcl->curvature / double(1000) - head->offset_time; /* Transform to the 'end' frame */ M3D R_i(R_imu * Exp(angvel_avr, dt)); V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - state_inout.pos_end); V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); // V3D P_compensate = Lid_rot_to_IMU.transpose() * // (state_inout.rot_end.transpose() * (R_i * (Lid_rot_to_IMU * P_i + // Lid_offset_to_IMU) + T_ei) - Lid_offset_to_IMU); V3D P_compensate = (extR_Ri * (R_i * (Lid_rot_to_IMU * P_i + Lid_offset_to_IMU) + T_ei) - exrR_extT); /// save Undistorted points and their rotation it_pcl->x = P_compensate(0); it_pcl->y = P_compensate(1); it_pcl->z = P_compensate(2); if (it_pcl == pcl_wait_proc.points.begin()) break; } } pcl_out = pcl_wait_proc; pcl_wait_proc.clear(); IMUpose.clear(); } // printf("[ IMU ] time forward: %lf, backward: %lf.\n", t1 - t0, omp_get_wtime() - t1); } void ImuProcess::Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_) { double t1, t2, t3; t1 = omp_get_wtime(); ROS_ASSERT(lidar_meas.lidar != nullptr); if (!imu_en) { Forward_without_imu(lidar_meas, stat, *cur_pcl_un_); return; } MeasureGroup meas = lidar_meas.measures.back(); if (imu_need_init) { double pcl_end_time = lidar_meas.lio_vio_flg == LIO ? meas.lio_time : meas.vio_time; // lidar_meas.last_lio_update_time = pcl_end_time; if (meas.imu.empty()) { return; }; /// The very first lidar frame IMU_init(meas, stat, init_iter_num); imu_need_init = true; last_imu = meas.imu.back(); if (init_iter_num > MAX_INI_COUNT) { // cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); imu_need_init = false; ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; acc covarience: " "%.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f \n", stat.gravity[0], stat.gravity[1], stat.gravity[2], mean_acc.norm(), cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]); ROS_INFO("IMU Initials: ba covarience: %.8f %.8f %.8f; bg covarience: " "%.8f %.8f %.8f", cov_bias_acc[0], cov_bias_acc[1], cov_bias_acc[2], cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2]); fout_imu.open(DEBUG_FILE_DIR("imu.txt"), ios::out); } return; } UndistortPcl(lidar_meas, stat, *cur_pcl_un_); // cout << "[ IMU ] undistorted point num: " << cur_pcl_un_->size() << endl; }请帮我找到卡尔曼增益矩阵的具体位置。IMU_Processing.cpp文件内容如上
07-17
<think>根据用户提供的引用[1]中的代码片段,我们可以看出卡尔曼增益矩阵(通常用$K$表示)的计算公式为: $$ K = P H^T (H P H^T + R)^{-1} $$ 其中: - $P$ 是状态协方差矩阵(在代码中可能为`Cov_`) - $H$ 是观测矩阵 - $R$ 是观测噪声协方差矩阵 在`IMU_Processing.cpp`文件中,我们可以按照以下步骤定位卡尔曼增益矩阵的实现位置: 1. **查找关键变量名**: - 协方差矩阵:通常命名为`Cov_`, `P_`, `covariance`等 - 观测矩阵:通常命名为`H_`, `H_matrix`等 - 观测噪声矩阵:通常命名为`R_`, `R_matrix`等 - 卡尔曼增益:通常命名为`K_`, `K`, `KalmanGain`等 2. **查找关键计算表达式**: - 矩阵求逆:如`.inverse()`或`inv()` - 矩阵乘法:如`*`运算符 - 表达式结构:`Cov_ * H.transpose() * (H * Cov_ * H.transpose() + R).inverse()` 3. **查找状态更新代码块**: - 通常在函数名包含`update`, `correct`, `filter`等字眼的函数中 4. **注意代码上下文**: - 卡尔曼增益计算后通常紧跟着状态更新和协方差更新 ### 具体定位建议 在`IMU_Processing.cpp`中,建议使用文本搜索以下关键词: - `KalmanGain` - `K_` - `.inverse()` - `H.transpose()` - `Cov_` 或 `P_` ### 预期代码位置 根据引用[1]的示例,你可能会找到类似以下代码片段: ```cpp // 计算卡尔曼增益 Eigen::MatrixXd temp = H * Cov_ * H.transpose() + R; Eigen::MatrixXd K = Cov_ * H.transpose() * temp.inverse(); // 卡尔曼增益矩阵计算[^1] // 状态更新 dx_ = dx_ + K * (dz - H * dx_); // 协方差更新 Cov_ = (I - K * H) * Cov_; // 或使用其他形式 ``` ### 注意事项 - 实际代码中变量名可能不同(如协方差矩阵可能命名为`state_cov`) - 可能使用其他求逆方法(如Cholesky分解)以提高数值稳定性 - 在迭代卡尔曼滤波(IEKF)中,该计算可能位于循环内 ### 推荐搜索步骤 1. 全局搜索`inverse(`(注意带括号) 2. 搜索`H.transpose()`(观测矩阵转置) 3. 搜索`K =`(卡尔曼增益赋值) ### 相关问题 1. 在卡尔曼滤波中,除了直接求逆,还有哪些方法可以计算卡尔曼增益以提高数值稳定性? 2. 如何验证卡尔曼增益矩阵的计算是否正确? 3. 当观测维度很高时,计算卡尔曼增益的复杂度如何优化?</think>根据您提供的引用[1]中的卡尔曼增益计算公式和典型实现模式,在`IMU_Processing.cpp`文件中定位卡尔曼增益矩阵的实现位置,建议查找以下关键代码模式: ### 定位步骤 1. **查找核心计算公式**: ```cpp // 核心计算公式(参考引用[1]) auto temp = H * Cov_ * H.transpose() + R; Eigen::MatrixXd K = Cov_ * H.transpose() * temp.inverse(); // 卡尔曼增益矩阵计算[^1] ``` 2. **查找状态更新代码块**: ```cpp // 状态更新(通常紧随增益计算) dx_ = dx_ + K * (dz - H * dx_); // 状态修正 ``` 3. **查找协方差更新**: ```cpp // 协方差更新(Joseph形式或标准形式) Cov_ = (I - K * H) * Cov_; // 或 Cov_ = (I - K*H)*P_pred; ``` ### 具体位置建议 在文件中搜索以下关键词: 1. **矩阵求逆操作**: - `.inverse()`(如`temp.inverse()`) - `ldlt().solve()`(更稳定的求解方式) 2. **矩阵乘法链**: - `Cov_ * H.transpose() * (...)` - `P_ * H_T * S_inv`(P:协方差,H:观测矩阵,S:新息协方差) 3. **典型变量名**: - 卡尔曼增益:`K`, `K_`, `kalman_gain` - 协方差:`Cov_`, `P_`, `state_cov` - 观测矩阵:`H_`, `H_matrix` ### 预期上下文 通常在以下函数中: ```cpp void updateState(const Eigen::VectorXd& z_meas) { // ...预测步骤... // 计算卡尔曼增益(核心位置) Eigen::MatrixXd S = H_ * P_pred_ * H_.transpose() + R_; Eigen::MatrixXd K = P_pred_ * H_.transpose() * S.inverse(); // 查找此模式[^1] // 状态更新 x_ += K * (z_meas - H_ * x_pred_); // 协方差更新 P_ = (I_ - K * H_) * P_pred_; } ``` ### 注意事项 1. 现代实现可能使用**数值稳定形式**: ```cpp // 使用QR分解或Cholesky分解代替直接求逆 K = P_pred_ * H_.transpose() * S.ldlt().solve(I); ``` 2. 可能出现在**误差状态更新**中(如ESKF): ```cpp // 误差状态更新 Eigen::VectorXd dx = K * dz; error_state_ += dx; ``` > 建议使用全局搜索: > `grep -n "inverse()" IMU_Processing.cpp` > `grep -n "K =" IMU_Processing.cpp`
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值