fangxiangduile

assets/camera_calib/1.txt

@@ -0,0 +1 @@
+0.7177201718666837 0.09187038370024973 0.09699144216061435 -0.6833961132769422

assets/camera_calib/10.txt

@@ -0,0 +1 @@
+0.6540393670774336 0.11673220229558735 0.104104730295333 -0.74011371044611

assets/camera_calib/11.txt

@@ -0,0 +1 @@
+0.6695365068706467 0.11396525651605409 0.10689676647567184 -0.7261582937567594

assets/camera_calib/12.txt

@@ -0,0 +1 @@
+0.6865027206257972 0.11121980925498735 0.10959354583553703 -0.7101643635904696

assets/camera_calib/13.txt

@@ -0,0 +1 @@
+0.7029618311310923 0.10910831122785473 0.114008892573956 -0.693499828989083

assets/camera_calib/14.txt

@@ -0,0 +1 @@
+0.6846990959849547 0.11378336536460135 0.1095621509728064 -0.7115030771528001

assets/camera_calib/15.txt

@@ -0,0 +1 @@
+0.7105886427866503 0.09454929951290965 0.09772990470338523 -0.6903427238922067

assets/camera_calib/16.txt

@@ -0,0 +1 @@
+0.7082004773279313 0.09457995680211755 0.09737030792091227 -0.6928388981712751

assets/camera_calib/17.txt

@@ -0,0 +1 @@
+0.6864091145945169 0.08696336166250927 0.08462036719328926 -0.7170211256204378

assets/camera_calib/18.txt

@@ -0,0 +1 @@
+0.6667428632979302 0.10364170157370939 0.0955403079117792 -0.7318363215229934

assets/camera_calib/19.txt

@@ -0,0 +1 @@
+0.6608952401030105 0.0924475886118082 0.08181877213214009 -0.7402544248404296

assets/camera_calib/2.txt

@@ -0,0 +1 @@
+0.7258014151886103 0.06893544090588102 0.07336676467256588 -0.6804980003929494

assets/camera_calib/20.txt

@@ -0,0 +1 @@
+0.6368114936681568 0.09604567438504773 0.07995934840288652 -0.7608237986334537

assets/camera_calib/21.txt

@@ -0,0 +1 @@
+0.6572286496356463 0.07285829130720428 0.06370958053012925 -0.7474511762212797

assets/camera_calib/22.txt

@@ -0,0 +1 @@
+0.6350787937658589 0.07596458908813118 0.06184461820780068 -0.7662111654839704

assets/camera_calib/3.txt

@@ -0,0 +1 @@
+0.7153160451022573 0.06679164379924944 0.06815692277445147 -0.6922546249869169

assets/camera_calib/4.txt

@@ -0,0 +1 @@
+0.6895526075414257 0.07227717802997588 0.06701324523260725 -0.7174973421081351

assets/camera_calib/5.txt

@@ -0,0 +1 @@
+0.6755160976098915 0.06749445326901966 0.0605731820592963 -0.7317468074840031

assets/camera_calib/6.txt

@@ -0,0 +1 @@
+0.6554968212916839 0.0710136384754573 0.06108937801660896 -0.7493657773881318

assets/camera_calib/7.txt

@@ -0,0 +1 @@
+0.6453150221921371 0.08754614426431993 0.07488900209128711 -0.7551793377227582

assets/camera_calib/8.txt

@@ -0,0 +1 @@
+0.6436936453206352 0.1018123504027431 0.08674315464388545 -0.7535040553319725

assets/camera_calib/9.txt

@@ -0,0 +1 @@
+0.6409116406794043 0.11242770296004105 0.09554048153994232 -0.7533022612709181

calibration/capture.cpp

@@ -39,10 +39,10 @@ void capture_loop(
 
     // 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂
     auto img_with_ypr = img.clone();
-    Eigen::Vector3d zyx = tools::eulers(q, 2, 1, 0) * 57.3;  // degree
+    Eigen::Vector3d zyx = tools::eulers(q, 2, 1, 0);  // degree
-    tools::draw_text(img_with_ypr, fmt::format("Z {:.2f}", zyx[0]), {40, 40}, {0, 0, 255});
+    tools::draw_text(img_with_ypr, fmt::format("Yaw   (Z) {:.2f}", zyx[0]), {40, 40}, {0, 0, 255});
-    tools::draw_text(img_with_ypr, fmt::format("Y {:.2f}", zyx[1]), {40, 80}, {0, 0, 255});
+    tools::draw_text(img_with_ypr, fmt::format("Pitch (Y) {:.2f}", zyx[1]), {40, 80}, {0, 0, 255});
-    tools::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255});
+    tools::draw_text(img_with_ypr, fmt::format("Roll  (X) {:.2f}", zyx[2]), {40, 120}, {0, 0, 255});
 
     std::vector<cv::Point2f> centers_2d;
     auto success = cv::findCirclesGrid(img, cv::Size(10, 7), centers_2d);  // 默认是对称圆点图案

configs/calibration.yaml

@@ -1,15 +1,23 @@
 pattern_cols: 10
 pattern_rows: 7
-center_distance_mm: 40
+center_distance_mm: 39
 
-R_gimbal2imubody: [1, 0, 0, 0, 1, 0,  0, 0, 1]
+# 重投影误差: 0.0833px
+camera_matrix: [1824.3445767434343, 0, 745.88898611634079, 0, 1828.9000780816298, 584.45884364277663, 0, 0, 1]
+distort_coeffs: [-0.084922638867033884, 0.22051971879198334, -0.00021700148630802023, 0.00043485717343934131, 0]
+
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+# 相机同理想情况的偏角: yaw174.40 pitch8.20 roll178.20 degree
+R_camera2gimbal: [0.093296993396547734, 0.14503826005617809, -0.9850175501700672, -0.99514483056044212, -0.017561170025079953, -0.096841992524828474, -0.03134385477239883, 0.98927018980015091, 0.14269567036452102]
+t_camera2gimbal: [-0.11903236791933375, -0.012143601357090483, -0.023682825149991701]
 
 #####-----gimbal参数-----#####
-com_port: "/dev/ttyACM1"
+com_port: "/dev/ttyACM0"
 
 camera_name: "hikrobot"
-exposure_ms: 3
+exposure_ms: 50
-gain: 20.0
+gain: 50.0
 vid_pid: "2bdf:0001"
 
 #####-----cboard参数-----#####

configs/handeye.yaml

@@ -0,0 +1,30 @@
+# 手眼标定测试配置文件
+# 该配置用于测试手眼标定结果的准确性
+
+# 测试网格参数
+height: 2.0        # 测试平面距离地面高度（米）
+grid_num: 5        # 网格数量（5x5网格）
+grid_size: 0.5     # 网格间距（米）
+delay: 15          # 时间补偿延迟（毫秒）
+
+# 相机内参（从标定结果复制）
+camera_matrix: [1824.3445767434343, 0, 745.88898611634079, 0, 1828.9000780816298, 584.45884364277663, 0, 0, 1]
+distort_coeffs: [-0.084922638867033884, 0.22051971879198334, -0.00021700148630802023, 0.00043485717343934131, 0]
+
+# 手眼标定结果（从标定结果复制）
+R_gimbal2imubody: [1, 0, 0, 0, -1, 0, 0, 0, 1]
+R_camera2gimbal: [0.093296993396547734, 0.14503826005617809, -0.9850175501700672, -0.99514483056044212, -0.017561170025079953, -0.096841992524828474, -0.03134385477239883, 0.98927018980015091, 0.14269567036452102]
+t_camera2gimbal: [-0.11903236791933375, -0.012143601357090483, -0.023682825149991701]
+
+# 相机参数
+camera_name: "hikrobot"
+exposure_ms: 50
+gain: 50.0
+vid_pid: "2bdf:0001"
+
+# 下位机参数
+com_port: "/dev/ttyACM0"
+quaternion_canid: 0x01
+bullet_speed_canid: 0x110
+send_canid: 0xff
+can_interface: "can0"

configs/standard3.yaml

@@ -61,15 +61,15 @@ vid_pid: "2bdf:0001"
 #  0  0  1
 R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
 
-# 重投影误差: 0.1820px
+# 重投影误差: 0.0833px
-camera_matrix: [1785.4881526822305, 0, 672.4806478241826, 0, 1785.026019470562, 559.89603224794314, 0, 0, 1]
+camera_matrix: [1824.3445767434343, 0, 745.88898611634079, 0, 1828.9000780816298, 584.45884364277663, 0, 0, 1]
-distort_coeffs: [-0.076005079619881746, 0.11182817466388446, 0.0005362204787722057, -0.0027546300984895122, 0]
+distort_coeffs: [-0.084922638867033884, 0.22051971879198334, -0.00021700148630802023, 0.00043485717343934131, 0]
 
-# 相机同理想情况的偏角: yaw1.44 pitch-7.28 roll0.96 degree
+# 相机同理想情况的偏角: yaw-2.19 pitch0.60 roll-1.22 degree
 # 标定板到世界坐标系原点的水平距离: 1.13 m
-# 标定板同竖直摆放时的偏角: yaw7.61 pitch13.92 roll-0.46 degree
+# 标定板同竖直摆放时的偏角: yaw-82.48 pitch10.19 roll-0.28 degree
-R_camera2gimbal: [-0.027182119030230909, -0.12616154330853446, 0.99163723074269183, -0.99949106557517331, 0.019998323121329122, -0.024853106601381177, -0.016695575474690555, -0.99180811252093692, -0.12664093215554434]
+R_camera2gimbal: [0.037945134408690975, 0.011210224413650311, 0.99921694223191693, -0.99905391724335679, -0.020835442782907131, 0.038172696587273909, 0.021247051922802046, -0.99972006841547179, 0.010409014936772491]
-t_camera2gimbal: [0.13160669975045827, 0.10377721766577375, 0.024908271912914642]
+t_camera2gimbal: [0.14266900667253352, 0.0061791593160151776, 0.051417922179356944]
 
 #####-----cboard参数-----#####
 quaternion_canid: 0x100
@@ -78,7 +78,7 @@ send_canid: 0xff
 can_interface: "can0"
 
 #####-----gimbal参数-----#####
-com_port: "/dev/gimbal"
+com_port: "/dev/ttyACM0"
 yaw_kp: 0
 yaw_kd: 0
 pitch_kp: 0

io/gimbal/gimbal.cpp

@@ -163,12 +163,47 @@ void Gimbal::read_thread()
       error_count++;
       continue;
     }
+    float q0 = rx_data_.q[0], q1 = rx_data_.q[1], q2 = rx_data_.q[2], q3 = rx_data_.q[3];
+    float yaw = rx_data_.yaw, yaw_vel = rx_data_.yaw_vel;
+    float pitch = rx_data_.pitch, pitch_vel = rx_data_.pitch_vel;
+    float bullet_speed = rx_data_.bullet_speed;
+    uint16_t bullet_count = rx_data_.bullet_count, crc16 = rx_data_.crc16;
+    
+    if (!tools::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
+      tools::logger()->debug("[Gimbal] CRC16 check failed.");
+      tools::logger()->debug(
+        "[Gimbal] Received crc16: {}, Calculated crc16: {}",
+        crc16,
+        tools::get_crc16(
+          reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_) - sizeof(rx_data_.crc16)));
+      continue;
+    }
+    /*打印受到的数据*/
+// struct __attribute__((packed)) GimbalToVision
+// {
+//   uint8_t head[2] = {'M', 'R'};
+//   uint8_t mode;  // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+//   float q[4];    // wxyz顺序
+//   float yaw;
+//   float yaw_vel;
+//   float pitch;
+//   float pitch_vel;
+//   float bullet_speed;
+//   uint16_t bullet_count;  // 子弹累计发送次数
+//   uint16_t crc16;
+// };
 
-    // if (!tools::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
+    // float q0 = rx_data_.q[0], q1 = rx_data_.q[1], q2 = rx_data_.q[2], q3 = rx_data_.q[3];
-    //   tools::logger()->debug("[Gimbal] CRC16 check failed.");
+    // float yaw = rx_data_.yaw, yaw_vel = rx_data_.yaw_vel;
-    //   continue;
+    // float pitch = rx_data_.pitch, pitch_vel = rx_data_.pitch_vel;
-    // }
+    // float bullet_speed = rx_data_.bullet_speed;
-
+    // uint16_t bullet_count = rx_data_.bullet_count, crc16 = rx_data_.crc16;
+    
+    tools::logger()->debug(
+      "[Gimbal] mode: {}, q: [{:.3f}, {:.3f}, {:.3f}, {:.3f}], yaw: {:.3f}, yaw_vel: {:.3f}, "
+      "pitch: {:.3f}, pitch_vel: {:.3f}, bullet_speed: {:.3f}, bullet_count: {}, crc16: {}",
+      rx_data_.mode, q0, q1, q2, q3, yaw, yaw_vel, pitch, pitch_vel, bullet_speed,
+      bullet_count, crc16);
     error_count = 0;
     Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]);
     queue_.push({q, t});

io/hikrobot/hikrobot.cpp

@@ -160,13 +160,19 @@ void HikRobot::capture_start()
         // Convert RGB to BGR
         cv::cvtColor(img, dst_image, cv::COLOR_RGB2BGR);
         img = dst_image;
+      } else if (is_color && pixel_type == PixelType_Gvsp_BGR8_Packed) {
+        // BGR format is already correct for OpenCV, no conversion needed
+        img = img;
       } else if (!is_color) {
         tools::logger()->warn("Unsupported pixel type: {:#x}. Camera may be outputting already converted format. Using raw image.", pixel_type);
         // If pixel format is not in the map, the image might already be in the desired format
         img = img;
       }
+      
+      // Swap R and B channels to fix color inversion (red becoming blue)
+      cv::cvtColor(img, img, cv::COLOR_BGR2RGB);
 
-      queue_.push({img, timestamp});
+      queue_.push({img.clone(), timestamp});
 
       ret = MV_CC_FreeImageBuffer(handle_, &raw);
       if (ret != MV_OK) {

kernel.errors.txt

@@ -0,0 +1,16 @@
+Instruction / Operand / Region Errors:
+
+/-------------------------------------------!!!KERNEL HEADER ERRORS FOUND!!!-------------------------------------------\
+Error in CISA routine with name: kernel
+                  Error Message: Input V38 = [256, 260) intersects with V37 = [256, 260)
+\----------------------------------------------------------------------------------------------------------------------/
+
+
+/-------------------------------------------!!!KERNEL HEADER ERRORS FOUND!!!-------------------------------------------\
+Error in CISA routine with name: kernel
+                  Error Message: Explicit input 2 must not follow an implicit input 0
+\----------------------------------------------------------------------------------------------------------------------/
+
+
+
+

true/1.txt

@@ -0,0 +1 @@
+0.7177201718666837 0.09187038370024973 0.09699144216061435 -0.6833961132769422

true/10.txt

@@ -0,0 +1 @@
+0.6540393670774336 0.11673220229558735 0.104104730295333 -0.74011371044611

true/11.txt

@@ -0,0 +1 @@
+0.6695365068706467 0.11396525651605409 0.10689676647567184 -0.7261582937567594

true/12.txt

@@ -0,0 +1 @@
+0.6865027206257972 0.11121980925498735 0.10959354583553703 -0.7101643635904696

true/13.txt

@@ -0,0 +1 @@
+0.7029618311310923 0.10910831122785473 0.114008892573956 -0.693499828989083

true/14.txt

@@ -0,0 +1 @@
+0.6846990959849547 0.11378336536460135 0.1095621509728064 -0.7115030771528001

true/15.txt

@@ -0,0 +1 @@
+0.7105886427866503 0.09454929951290965 0.09772990470338523 -0.6903427238922067

true/16.txt

@@ -0,0 +1 @@
+0.7082004773279313 0.09457995680211755 0.09737030792091227 -0.6928388981712751

true/17.txt

@@ -0,0 +1 @@
+0.6864091145945169 0.08696336166250927 0.08462036719328926 -0.7170211256204378

true/18.txt

@@ -0,0 +1 @@
+0.6667428632979302 0.10364170157370939 0.0955403079117792 -0.7318363215229934

true/19.txt

@@ -0,0 +1 @@
+0.6608952401030105 0.0924475886118082 0.08181877213214009 -0.7402544248404296

true/2.txt

@@ -0,0 +1 @@
+0.7258014151886103 0.06893544090588102 0.07336676467256588 -0.6804980003929494

true/20.txt

@@ -0,0 +1 @@
+0.6368114936681568 0.09604567438504773 0.07995934840288652 -0.7608237986334537

true/21.txt

@@ -0,0 +1 @@
+0.6572286496356463 0.07285829130720428 0.06370958053012925 -0.7474511762212797

true/22.txt

@@ -0,0 +1 @@
+0.6350787937658589 0.07596458908813118 0.06184461820780068 -0.7662111654839704

true/3.txt

@@ -0,0 +1 @@
+0.7153160451022573 0.06679164379924944 0.06815692277445147 -0.6922546249869169

true/4.txt

@@ -0,0 +1 @@
+0.6895526075414257 0.07227717802997588 0.06701324523260725 -0.7174973421081351

true/5.txt

@@ -0,0 +1 @@
+0.6755160976098915 0.06749445326901966 0.0605731820592963 -0.7317468074840031

true/6.txt

@@ -0,0 +1 @@
+0.6554968212916839 0.0710136384754573 0.06108937801660896 -0.7493657773881318

true/7.txt

@@ -0,0 +1 @@
+0.6453150221921371 0.08754614426431993 0.07488900209128711 -0.7551793377227582

true/8.txt

@@ -0,0 +1 @@
+0.6436936453206352 0.1018123504027431 0.08674315464388545 -0.7535040553319725

true/9.txt

@@ -0,0 +1 @@
+0.6409116406794043 0.11242770296004105 0.09554048153994232 -0.7533022612709181