Skip to content
TheChatArchiveThe Chat Archive
o1-preview-2024-09-12· OpenAIAi2 WildChat-4.8M (AI2)

下述的C++代码以及matlab代码实现了相同的作用,C++代码是matlab代码通过matlab coder工具转换而来,对比两种代码,详细解释C++代码的具体实现过程。…

  1. User

    下述的C++代码以及matlab代码实现了相同的作用,C++代码是matlab代码通过matlab coder工具转换而来,对比两种代码,详细解释C++代码的具体实现过程。 C++代码:
    d = b_uavD3DConn.AirSpeed; // UAV俯仰角限制下界 // UAV俯仰角限制上界 // 俯仰角计算 // 未归一化的方向向量 // 高度差 // 负高度为正方向 // 待跟踪航迹俯仰角不能超过UAV性能限制 // validateattributes(pitch, 'double', {">=", pitchLimit_1, "<=", // pitchLimit_2}); 航向角计算 目标轨线长度 方向向量 航迹采样 if (std::isnan(d)) { y.set_size(1, 1); y[0] = rtNaN; } else if ((d == 0.0) || (d < 0.0)) { y.set_size(1, 0); } else if (std::isinf(d)) { y.set_size(1, 1); y[0] = 0.0; } else if (std::floor(d) == d) { loop_ub = static_cast(4000.0 / d); y.set_size(1, loop_ub + 1); for (i = 0; i <= loop_ub; i++) { y[i] = d * static_cast(i); } } else { double apnd; ndbl = std::floor(4000.0 / d + 0.5); apnd = ndbl * d; if (d > 0.0) { cdiff = apnd - 4000.0; } else { cdiff = 4000.0 - apnd; } if (std::abs(cdiff) < 1.7763568394002505E-12) { ndbl++; apnd = 4000.0; } else if (cdiff > 0.0) { apnd = (ndbl - 1.0) * d; } else { ndbl++; } if (ndbl >= 0.0) { n = static_cast(ndbl); } else { n = 0; } y.set_size(1, n); if (n > 0) { y[0] = 0.0; if (n > 1) { y[n - 1] = apnd; nm1d2 = (n - 1) / 2; for (k = 0; k <= nm1d2 - 2; k++) { cdiff = (static_cast(k) + 1.0) * d; y[k + 1] = cdiff; y[(n - k) - 2] = apnd - cdiff; } if (nm1d2 << 1 == n - 1) { y[nm1d2] = apnd / 2.0; } else { cdiff = static_cast(nm1d2) * d; y[nm1d2] = cdiff; y[nm1d2 + 1] = apnd - cdiff; } } } } loop_ub = y.size(1); sample_LenSeq.set_size(y.size(1)); for (i = 0; i < loop_ub; i++) { sample_LenSeq[i] = y[i]; } // 目标航迹采样长度索引序列 // 目标航迹采样数量 // 航迹采样点不能超过10个 if (sample_LenSeq.size(0) > maxSampleNum) { unsigned int u; cdiff = std::floor(static_cast(sample_LenSeq.size(0)) / maxSampleNum); if (cdiff < 4.294967296E+9) { if (cdiff >= 0.0) { u = static_cast(cdiff); } else { u = 0U; } } else if (cdiff >= 4.294967296E+9) { u = MAX_uint32_T; } else { u = 0U; } if ((u == 0U) || ((u > 0U) && (static_cast(sample_LenSeq.size(0)) < 1U))) { i = 1; k = -1; } else { i = static_cast(u); k = sample_LenSeq.size(0) - 1; } loop_ub = div_s32(k, i); n = loop_ub + 1; b_sample_LenSeq.set_size(loop_ub + 1); for (k = 0; k <= loop_ub; k++) { b_sample_LenSeq[k] = sample_LenSeq[i * k]; } sample_LenSeq.set_size(loop_ub + 1); for (i = 0; i < n; i++) { sample_LenSeq[i] = b_sample_LenSeq[i]; } } loop_ub = sample_LenSeq.size(0); sample_Poses.set_size(sample_LenSeq.size(0), 4); n = sample_LenSeq.size(0) << 2; for (i = 0; i < n; i++) { sample_Poses[i] = 0.0; } // 显式定义为四列矩阵 n = (loop_ub / 2) << 1; nm1d2 = n - 2; for (i = 0; i < 3; i++) { for (k = 0; k <= nm1d2; k += 2) { __m128d r; r = _mm_loadu_pd(&sample_LenSeq[k]); _mm_storeu_pd( &sample_Poses[k + sample_Poses.size(0) * i], _mm_add_pd( _mm_mul_pd(_mm_set1_pd(static_cast(dir_vec[i])), r), mm_set1_pd(static_cast(startPos[i])))); } for (k = n; k < loop_ub; k++) { sample_Poses[k + sample_Poses.size(0) * i] = static_cast(dir_vec[i]) * sample_LenSeq[k] + static_cast(startPos[i]); } } // 前三列为位置线性采样 // sample_Poses = startPos + dir_vec .* sample_LenSeq; % // 目标轨线位置线性采样 保证最后一个线性插值点为终点 x_data[0] = (sample_Poses[sample_Poses.size(0) - 1] != 0.0); x_data[1] = (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0)) - 1] != 4000.0); x_data[2] = (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0) * 2) - 1] != -1000.0); b_y = false; k = 0; exitg1 = false; while ((!exitg1) && (k <= 2)) { if (x_data[k]) { b_y = true; exitg1 = true; } else { k++; } } if (b_y) { sample_Poses[sample_Poses.size(0)] = 0.0; sample_Poses[sample_Poses.size(0) + sample_Poses.size(0)] = 4000.0; sample_Poses[sample_Poses.size(0) + sample_Poses.size(0) * 2] = -1000.0; } n = sample_Poses.size(0); iobj_1.sumple_num = n; for (i = 0; i < n; i++) { sample_Poses[i + sample_Poses.size(0) * 3] = 0.0; } // 初始化第四列 for (i = 0; i < n; i++) { sample_Poses[i + sample_Poses.size(0) * 3] = 1.5707963267948966; } matlab代码: % 用直线构造目标航迹并采样航迹点位姿 function [sample_Poses, sample_Num, target_PathSeg] = Get_TargetPathSeg_Line(~, uavD3DConn, target_PathPose, maxSampleNum) % 说明: % 所有Get_TargetPathSeg*方法拥有相同的输入输出接口 % 输入: % 输出:

        % 输入获取
        airSpeed        = uavD3DConn.AirSpeed;
        pitchLimit_1    = uavD3DConn.FlightPathAngleLimit(1);   % UAV俯仰角限制下界
        pitchLimit_2    = uavD3DConn.FlightPathAngleLimit(2);   % UAV俯仰角限制上界
        startPos        = target_PathPose(1, 1:3);
        goalPos         = target_PathPose(2, 1:3);
        % 俯仰角计算
        delta_XYZ   = goalPos - startPos;                       % 未归一化的方向向量
        delta_Z     = delta_XYZ(3);                             % 高度差
        pitch       = atan(-delta_Z / norm(delta_XYZ(1:2)));    % 负高度为正方向
        % 待跟踪航迹俯仰角不能超过UAV性能限制
        % validateattributes(pitch, 'double', {">=", pitchLimit_1, "<=", pitchLimit_2});
        % 航向角计算
        target_PathLength   = norm(delta_XYZ);                  % 目标轨线长度
        dir_vec             = (delta_XYZ) / target_PathLength;  % 方向向量
        heading             = CalConvergencePt.Get_NED_Heading(dir_vec(1:2));
        % 航迹采样
        sample_LenSeq   = (0 : airSpeed : target_PathLength)';  % 目标航迹采样长度索引序列
        sample_Num      = length(sample_LenSeq);                % 目标航迹采样数量
        % 航迹采样点不能超过10个
        if sample_Num > maxSampleNum
            sampleLenInterval   = uint32(floor(sample_Num / maxSampleNum));
            sample_LenSeq       = sample_LenSeq(1 : sampleLenInterval : sample_Num);
        end
        sample_Poses = zeros(length(sample_LenSeq), 4);  % 显式定义为四列矩阵
        sample_Poses(:, 1:3) = startPos + dir_vec .* sample_LenSeq;  % 前三列为位置线性采样
        % sample_Poses = startPos + dir_vec .* sample_LenSeq;     % 目标轨线位置线性采样
        numRows = size(sample_Poses, 1);
        % 保证最后一个线性插值点为终点
        if any(sample_Poses(numRows, 1:3) ~= goalPos)
            sample_Poses(numRows+1, 1:3) = goalPos;
        end
        sample_Poses(:, 4)  = 0;                                % 初始化第四列
        sample_Poses(:, 4)  = heading;                                  % 线性采样点加入姿态
        sample_Num          = size(sample_Poses, 1);                    % 线性采样点数量
        target_PathSeg      = [startPos, heading; goalPos, heading];    % 起止目标航迹点位姿
    
  2. Assistant

    本文提供的C++代码是通过MATLAB Coder工具将MATLAB代码转换而来,两个代码实现了相同的功能,即用直线构造目标航迹并采样航迹点位姿。下面将对C++代码进行详细的解释,并对照MATLAB代码,说明每一步的具体实现过程。


    1. 读取UAV的空速(AirSpeed)

    C++代码:

    d = b_uavD3DConn.AirSpeed;
    

    MATLAB代码:

    airSpeed = uavD3DConn.AirSpeed;
    

    这里的 d 对应于MATLAB代码中的 airSpeed,表示无人机的空速。


    2. 生成航迹采样长度序列 sample_LenSeq

    C++代码:

    if (std::isnan(d)) {
      y.set_size(1, 1);
      y[0] = rtNaN;
    } else if ((d == 0.0) || (d < 0.0)) {
      y.set_size(1, 0);
    } else if (std::isinf(d)) {
      y.set_size(1, 1);
      y[0] = 0.0;
    } else if (std::floor(d) == d) {
      loop_ub = static_cast<int>(4000.0 / d);
      y.set_size(1, loop_ub + 1);
      for (i = 0; i <= loop_ub; i++) {
        y[i] = d * static_cast<double>(i);
      }
    } else {
      // 处理步长为非整数的情况
      // ...(后续代码)
    }
    

    MATLAB代码:

    sample_LenSeq = (0 : airSpeed : target_PathLength)';
    

    在MATLAB中,使用冒号操作符 : 直接生成从0到目标路径长度 target_PathLength,以 airSpeed 为步长的序列。

    由于C++语言中没有直接的冒号操作符,需要通过编程实现相同的功能。C++代码中,对 d(即 airSpeed)进行了多种情况的判断:

    • NaN、零、负数、无穷大的情况,分别进行特殊处理。
    • 整数步长的情况,直接计算序列的长度 loop_ub = static_cast<int>(4000.0 / d);,然后生成序列。
    • 非整数步长的情况,进行精确的浮点数计算,避免累积误差。

    这里的 4000.0 对应于 target_PathLength,即目标路径的长度。


    3. 处理非整数步长的情况

    当步长 d 为非整数时,需要特别处理,以确保生成的序列与MATLAB中的结果一致。

    C++代码:

    double apnd;
    ndbl = std::floor(4000.0 / d + 0.5);
    apnd = ndbl * d;
    if (d > 0.0) {
      cdiff = apnd - 4000.0;
    } else {
      cdiff = 4000.0 - apnd;
    }
    if (std::abs(cdiff) < 1.7763568394002505E-12) {
      ndbl++;
      apnd = 4000.0;
    } else if (cdiff > 0.0) {
      apnd = (ndbl - 1.0) * d;
    } else {
      ndbl++;
    }
    if (ndbl >= 0.0) {
      n = static_cast<int>(ndbl);
    } else {
      n = 0;
    }
    y.set_size(1, n);
    if (n > 0) {
      y[0] = 0.0;
      if (n > 1) {
        y[n - 1] = apnd;
        nm1d2 = (n - 1) / 2;
        for (k = 0; k <= nm1d2 - 2; k++) {
          cdiff = (static_cast<double>(k) + 1.0) * d;
          y[k + 1] = cdiff;
          y[(n - k) - 2] = apnd - cdiff;
        }
        if (nm1d2 << 1 == n - 1) {
          y[nm1d2] = apnd / 2.0;
        } else {
          cdiff = static_cast<double>(nm1d2) * d;
          y[nm1d2] = cdiff;
          y[nm1d2 + 1] = apnd - cdiff;
        }
      }
    }
    

    这部分代码的作用是精确地计算非整数步长情况下的采样序列,避免浮点数累积误差,与MATLAB行为保持一致。


    4. 将生成的序列赋值给 sample_LenSeq

    C++代码:

    loop_ub = y.size(1);
    sample_LenSeq.set_size(y.size(1));
    for (i = 0; i < loop_ub; i++) {
      sample_LenSeq[i] = y[i];
    }
    

    相当于MATLAB中的:

    sample_LenSeq = y';  % 转置为列向量
    

    5. 限制采样点数量

    为防止采样点过多,代码限制了采样点的数量,确保不超过 maxSampleNum。

    C++代码:

    if (sample_LenSeq.size(0) > maxSampleNum) {
      cdiff = std::floor(static_cast<double>(sample_LenSeq.size(0)) / maxSampleNum);
      unsigned int u;
      if (cdiff < 4294967296.0) {
        if (cdiff >= 0.0) {
          u = static_cast<unsigned int>(cdiff);
        } else {
          u = 0U;
        }
      } else {
        u = 4294967295U;
      }
      if ((u == 0U) || ((u > 0U) && (static_cast<unsigned int>(sample_LenSeq.size(0)) < 1U))) {
        i = 1;
        k = -1;
      } else {
        i = static_cast<int>(u);
        k = sample_LenSeq.size(0) - 1;
      }
      loop_ub = div_s32(k, i);
      n = loop_ub + 1;
      b_sample_LenSeq.set_size(loop_ub + 1);
      for (k = 0; k <= loop_ub; k++) {
        b_sample_LenSeq[k] = sample_LenSeq[i * k];
      }
      sample_LenSeq.set_size(loop_ub + 1);
      for (i = 0; i < n; i++) {
        sample_LenSeq[i] = b_sample_LenSeq[i];
      }
    }
    

    对应的MATLAB代码:

    if sample_Num > maxSampleNum
        sampleLenInterval = uint32(floor(sample_Num / maxSampleNum));
        sample_LenSeq = sample_LenSeq(1 : sampleLenInterval : sample_Num);
    end
    

    这里,C++代码通过计算采样间隔 u,重新采样 sample_LenSeq,以控制采样点数量。


    6. 初始化并计算 sample_Poses

    C++代码:

    loop_ub = sample_LenSeq.size(0);
    sample_Poses.set_size(sample_LenSeq.size(0), 4);
    n = sample_LenSeq.size(0) << 2;
    for (i = 0; i < n; i++) {
      sample_Poses[i] = 0.0;
    }
    // 位置线性采样
    n = (loop_ub / 2) << 1;
    nm1d2 = n - 2;
    for (i = 0; i < 3; i++) {
      for (k = 0; k <= nm1d2; k += 2) {
        __m128d r;
        r = _mm_loadu_pd(&sample_LenSeq[k]);
        _mm_storeu_pd(
            &sample_Poses[k + sample_Poses.size(0) * i],
            _mm_add_pd(
                _mm_mul_pd(_mm_set1_pd(static_cast<double>(dir_vec[i])), r),
                _mm_set1_pd(static_cast<double>(startPos[i]))));
      }
      for (k = n; k < loop_ub; k++) {
        sample_Poses[k + sample_Poses.size(0) * i] =
            static_cast<double>(dir_vec[i]) * sample_LenSeq[k] +
            static_cast<double>(startPos[i]);
      }
    }
    

    对应的MATLAB代码:

    sample_Poses = zeros(length(sample_LenSeq), 4);  % 显式定义为四列矩阵
    sample_Poses(:, 1:3) = startPos + dir_vec .* sample_LenSeq;  % 前三列为位置线性采样
    

    C++代码中,使用了SIMD指令(如 __m128d)对 sample_Poses 的前三列进行计算,以加快处理速度。


    7. 确保最后一个采样点为终点

    C++代码:

    x_data[0] = (sample_Poses[sample_Poses.size(0) - 1] != 0.0);
    x_data[1] = (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0)) - 1] != 4000.0);
    x_data[2] = (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0) * 2) - 1] != -1000.0);
    b_y = false;
    k = 0;
    exitg1 = false;
    while ((!exitg1) && (k <= 2)) {
      if (x_data[k]) {
        b_y = true;
        exitg1 = true;
      } else {
        k++;
      }
    }
    if (b_y) {
      sample_Poses[sample_Poses.size(0)] = 0.0;
      sample_Poses[sample_Poses.size(0) + sample_Poses.size(0)] = 4000.0;
      sample_Poses[sample_Poses.size(0) + sample_Poses.size(0) * 2] = -1000.0;
    }
    

    对应的MATLAB代码:

    numRows = size(sample_Poses, 1);
    if any(sample_Poses(numRows, 1:3) ~= goalPos)
        sample_Poses(numRows+1, 1:3) = goalPos;
    end
    

    C++代码通过检查 sample_Poses 最后一行的前三个元素是否等于终点坐标(在此假设为 (0, 4000, -1000)),如果不等,则在 sample_Poses 中追加终点坐标,确保最后一个采样点为终点。


    8. 初始化并赋值第四列(航向角)

    C++代码:

    n = sample_Poses.size(0);
    for (i = 0; i < n; i++) {
      sample_Poses[i + sample_Poses.size(0) * 3] = 0.0;
    }
    // 初始化第四列
    for (i = 0; i < n; i++) {
      sample_Poses[i + sample_Poses.size(0) * 3] = 1.5707963267948966;
    }
    

    对应的MATLAB代码:

    sample_Poses(:, 4) = 0;          % 初始化第四列
    sample_Poses(:, 4) = heading;    % 线性采样点加入姿态
    

    在C++代码中,使用了 1.5707963267948966,即 π/2,表示航向角为90度(弧度制)。在MATLAB代码中,heading 是通过计算得到的航向角。


    9. 总结

    通过对比分析,可以看出C++代码严格按照MATLAB代码的逻辑进行了实现,主要包括以下步骤:

    • 读取输入参数:如空速、起始和结束位置等。
    • 生成采样长度序列:考虑了特殊情况,模拟了MATLAB的冒号操作符。
    • 限制采样点数量:确保采样点数量不超过最大值。
    • 计算采样点位姿:使用向量化计算方法,高效地计算采样点的位置和姿态。
    • 确保采样点包含终点:检查最后一个采样点是否为终点,如果不是,则追加终点。
    • 赋值航向角:初始化并设置每个采样点的航向角。

    需要注意的是,由于语言特性的差异,MATLAB中的一些简洁操作在C++中需要更复杂的实现。此外,C++代码中使用了SIMD指令进行优化,提高了计算效率。


    总体而言,C++代码实现了MATLAB代码的所有功能,对每一步骤都进行了详细的处理,确保在各种情况下都能得到与MATLAB相同的结果。