show_prop.cpp

本文提供了一个Windows应用程序示例,展示了如何使用SetProp和GetProp函数来为窗口添加和检索属性值。通过简单的实例,读者可以了解到如何在窗口上设置字符串类型的属性,并在消息响应中获取这些属性。

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

  name="google_ads_frame" marginwidth="0" marginheight="0" src="http://pagead2.googlesyndication.com/pagead/ads?client=ca-pub-5572165936844014&dt=1194442938015&lmt=1194190197&format=336x280_as&output=html&correlator=1194442937843&url=file%3A%2F%2F%2FC%3A%2FDocuments%2520and%2520Settings%2Flhh1%2F%E6%A1%8C%E9%9D%A2%2FCLanguage.htm&color_bg=FFFFFF&color_text=000000&color_link=000000&color_url=FFFFFF&color_border=FFFFFF&ad_type=text&ga_vid=583001034.1194442938&ga_sid=1194442938&ga_hid=1942779085&flash=9&u_h=768&u_w=1024&u_ah=740&u_aw=1024&u_cd=32&u_tz=480&u_java=true" frameborder="0" width="336" scrolling="no" height="280" allowtransparency="allowtransparency"> 
#include <windows.h> 
#include "show_prop.h"


#if defined (WIN32)
 #define IS_WIN32 TRUE
#else
 #define IS_WIN32 FALSE
#endif

#define IS_NT      IS_WIN32 && (BOOL)(GetVersion() < 0x80000000)
#define IS_WIN32S  IS_WIN32 && (BOOL)(!(IS_NT) && (LOBYTE(LOWORD(GetVersion()))<4))
#define IS_WIN95   (BOOL)(!(IS_NT) && !(IS_WIN32S)) && IS_WIN32

HINSTANCE hInst;   // current instance

LPCTSTR lpszAppName  = "MyApp";
LPCTSTR lpszTitle    = "Show_Prop";

BOOL RegisterWin95( CONST WNDCLASS* lpwc );

int APIENTRY WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance,
                      LPTSTR lpCmdLine, int nCmdShow)
{
   MSG      msg;
   HWND     hWnd;
   WNDCLASS wc;

   // Register the main application window class.
   //............................................
   wc.style         = CS_HREDRAW | CS_VREDRAW;
   wc.lpfnWndProc   = (WNDPROC)WndProc;      
   wc.cbClsExtra    = 0;                     
   wc.cbWndExtra    = 0;                     
   wc.hInstance     = hInstance;             
   wc.hIcon         = LoadIcon( hInstance, lpszAppName );
   wc.hCursor       = LoadCursor(NULL, IDC_ARROW);
   wc.hbrBackground = (HBRUSH)(COLOR_WINDOW+1);
   wc.lpszMenuName  = lpszAppName;             
   wc.lpszClassName = lpszAppName;             

   if ( IS_WIN95 )
   {
      if ( !RegisterWin95( &wc ) )
         return( FALSE );
   }
   else if ( !RegisterClass( &wc ) )
      return( FALSE );

   hInst = hInstance;

   // Create the main application window.
   //....................................
   hWnd = CreateWindow( lpszAppName,
                        lpszTitle,   
                        WS_OVERLAPPEDWINDOW,
                        CW_USEDEFAULT, 0,
                        CW_USEDEFAULT, 0, 
                        NULL,             
                        NULL,             
                        hInstance,        
                        NULL              
                      );

   if ( !hWnd )
      return( FALSE );

   ShowWindow( hWnd, nCmdShow );
   UpdateWindow( hWnd );        

   while( GetMessage( &msg, NULL, 0, 0) )  
   {
      TranslateMessage( &msg );
      DispatchMessage( &msg ); 
   }

   return( msg.wParam );
}


BOOL RegisterWin95( CONST WNDCLASS* lpwc )
{
   WNDCLASSEX wcex;

   wcex.style         = lpwc->style;
   wcex.lpfnWndProc   = lpwc->lpfnWndProc;
   wcex.cbClsExtra    = lpwc->cbClsExtra;
   wcex.cbWndExtra    = lpwc->cbWndExtra;
   wcex.hInstance     = lpwc->hInstance;
   wcex.hIcon         = lpwc->hIcon;
   wcex.hCursor       = lpwc->hCursor;
   wcex.hbrBackground = lpwc->hbrBackground;
   wcex.lpszMenuName  = lpwc->lpszMenuName;
   wcex.lpszClassName = lpwc->lpszClassName;

   // Added elements for Windows 95.
   //...............................
   wcex.cbSize = sizeof(WNDCLASSEX);
   wcex.hIconSm = LoadImage(wcex.hInstance, lpwc->lpszClassName,
                            IMAGE_ICON, 16, 16,
                            LR_DEFAULTCOLOR );
   
   return RegisterClassEx( &wcex );
}

LRESULT CALLBACK WndProc( HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam )
{
static LPCTSTR szProp = "Value for Property";

   switch( uMsg )
   {
      case WM_CREATE :
               // Add a property item that contains a string
               // pointer to the main window.
               //................................................
               SetProp( hWnd, "Property ID", (HANDLE)szProp );
               break;

      case WM_COMMAND :
              switch( LOWORD( wParam ) )
              {
                 case IDM_TEST :
                       // Get the value of the property added to the window
                       // and display it in a message box.
                       //..................................................
                       MessageBox( NULL,
                                   (LPCTSTR)GetProp( hWnd,"Property ID" ),
                                   "Property Value", MB_OK );
                       break;

                 case IDM_ABOUT :
                        DialogBox( hInst, "AboutBox", hWnd, (DLGPROC)About );
                        break;

                 case IDM_EXIT :
                        DestroyWindow( hWnd );
                        break;
              }
              break;
     
      case WM_DESTROY :
              // Remove the property item from the window. Since we
              // didn抰 allocate any memory for the property we don抰
              // need to free anything.
              //................................
              RemoveProp( hWnd, "Property ID" );
              PostQuitMessage(0);
              break;

      default :
            return( DefWindowProc( hWnd, uMsg, wParam, lParam ) );
   }

   return( 0L );
}


LRESULT CALLBACK About( HWND hDlg,          
                        UINT message,       
                        WPARAM wParam,      
                        LPARAM lParam)
{
   switch (message)
   {
       case WM_INITDIALOG:
               return (TRUE);

       case WM_COMMAND:                             
               if (   LOWORD(wParam) == IDOK        
                   || LOWORD(wParam) == IDCANCEL)   
               {
                       EndDialog(hDlg, TRUE);       
                       return (TRUE);
               }
               break;
   }

   return (FALSE);
}

/* 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
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值