some doubts and bugs

[LeegoChen]

一些疑惑和bug，如有错误，请您谅解。
1.void VelocityData::TransformCoordinate(Eigen::Matrix4f transform_matrix)
{
....

    Eigen::Vector3d r(matrix(0,3), matrix(1,3), matrix(2,3));
    Eigen::Vector3d delta_v;
    delta_v(0) = w(1) * r(2) - w(2) * r(1);
    delta_v(1) = w(2) * r(0) - w(0) * r(2);
    **delta_v(2) = w(1) * r(1) - w(1) * r(0);** //这里叉乘感觉 应该是w(0) * r(1) - w(1) * r(0)
    v = v + delta_v;

....
}

2. data_pretreat_flow.cpp 中

bool DataPretreatFlow::TransformData() {
    gnss_pose_ = Eigen::Matrix4f::Identity();

    current_gnss_data_.UpdateXYZ();
    gnss_pose_(0,3) = current_gnss_data_.local_E;
    gnss_pose_(1,3) = current_gnss_data_.local_N;
    gnss_pose_(2,3) = current_gnss_data_.local_U;
    gnss_pose_.block<3,3>(0,0) = current_imu_data_.GetOrientationMatrix();
    gnss_pose_ *= lidar_to_imu_;

    **current_velocity_data_.TransformCoordinate(lidar_to_imu_);**//该变换感觉是反了，是否应该改为lidar_to_imu_.reverse().
    distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);
    distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);

    return true;
}

3.distortion_adjust.cpp

bool DistortionAdjust::AdjustCloud(...)
{
    float orientation_space = 2.0 * M_PI;
    float delete_space = 5.0 * M_PI / 180.0;
    float start_orientation = atan2(origin_cloud_ptr->points[0].y, origin_cloud_ptr->points[0].x);

    Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ());
    Eigen::Matrix3f rotate_matrix = t_V.matrix();
    Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();
    transform_matrix.block<3,3>(0,0) = rotate_matrix.inverse();
   pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix);// 这边为什么要将 首个激光点 旋转到 雷达的X轴上去呢？
    velocity_ = rotate_matrix * velocity_;// 没能明白您这里的线速度和角速度为什么还需要坐标变换，您能解释下吗，麻烦了。
    angular_rate_ = rotate_matrix * angular_rate_;

    for (size_t point_index = 1; point_index < origin_cloud_ptr->points.size(); ++point_index) {
        float orientation = atan2(origin_cloud_ptr->points[point_index].y, origin_cloud_ptr->points[point_index].x);
        if (orientation < 0.0)
            orientation += 2.0 * M_PI;
        
        if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space)
            continue;

        float real_time = fabs(orientation) / orientation_space * scan_period_ - scan_period_ / 2.0;

        Eigen::Vector3f origin_point(origin_cloud_ptr->points[point_index].x,
                                     origin_cloud_ptr->points[point_index].y,
                                     origin_cloud_ptr->points[point_index].z);

        Eigen::Matrix3f current_matrix = UpdateMatrix(real_time);
        Eigen::Vector3f rotated_point = current_matrix * origin_point;
        Eigen::Vector3f adjusted_point = rotated_point + velocity_ * real_time;
        CloudData::POINT point;
        point.x = adjusted_point(0);
        point.y = adjusted_point(1);
        point.z = adjusted_point(2);
        output_cloud_ptr->points.push_back(point);
    }

    pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, transform_matrix.inverse());
    return true;
}

[Little-Potato-1990]

你提的第1和第2点都是对的，之前在知乎和大家讨论时也发现了这些问题，不过github上程序没改，太懒 关于补畸变的事情，别的程序里的流程都是先找到起始点和终止点，然后检测角度是否旋转过半(halfpassed那个变量)，在一帧点云里，如果点的排列顺序是先列后行，那么没有问题，只需要判断一次halfpassed就可以了，但是如果是先行后列，则需要判断64次，这个数据集里就属于后者，64次判断很难保证比较准，因为某一根线的部分区域没有返回值是很正常的现象。如果都转到第一个点上去，那么就可以根据每个点所处的象限判断它的角度了，不需要判断halfpassed（可能解释起来比较费劲，如果没解释清楚，可以加微信15311860904）

…

------------------&nbsp;原始邮件&nbsp;------------------ 发件人:&nbsp;"FrogyChen"<[email protected]&gt;; 发送时间:&nbsp;2020年6月12日(星期五) 晚上10:54 收件人:&nbsp;"Little-Potato-1990/localization_in_auto_driving"<[email protected]&gt;; 抄送:&nbsp;"Subscribed"<[email protected]&gt;; 主题:&nbsp;[Little-Potato-1990/localization_in_auto_driving] some doubts and bugs (#12) 一些疑惑和bug，如有错误，请您谅解。 1.void VelocityData::TransformCoordinate(Eigen::Matrix4f transform_matrix) { .... Eigen::Vector3d r(matrix(0,3), matrix(1,3), matrix(2,3)); Eigen::Vector3d delta_v; delta_v(0) = w(1) * r(2) - w(2) * r(1); delta_v(1) = w(2) * r(0) - w(0) * r(2); **delta_v(2) = w(1) * r(1) - w(1) * r(0);** //这里叉乘感觉 应该是w(0) * r(1) - w(1) * r(0) v = v + delta_v; .... } data_pretreat_flow.cpp 中 bool DataPretreatFlow::TransformData() { gnss_pose_ = Eigen::Matrix4f::Identity(); current_gnss_data_.UpdateXYZ(); gnss_pose_(0,3) = current_gnss_data_.local_E; gnss_pose_(1,3) = current_gnss_data_.local_N; gnss_pose_(2,3) = current_gnss_data_.local_U; gnss_pose_.block<3,3&gt;(0,0) = current_imu_data_.GetOrientationMatrix(); gnss_pose_ *= lidar_to_imu_; **current_velocity_data_.TransformCoordinate(lidar_to_imu_);**//该变换感觉是反了，是否应该改为lidar_to_imu_.reverse(). distortion_adjust_ptr_-&gt;SetMotionInfo(0.1, current_velocity_data_); distortion_adjust_ptr_-&gt;AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr); return true; } 3.distortion_adjust.cpp bool DistortionAdjust::AdjustCloud(...) { float orientation_space = 2.0 * M_PI; float delete_space = 5.0 * M_PI / 180.0; float start_orientation = atan2(origin_cloud_ptr-&gt;points[0].y, origin_cloud_ptr-&gt;points[0].x); Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ()); Eigen::Matrix3f rotate_matrix = t_V.matrix(); Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity(); transform_matrix.block<3,3&gt;(0,0) = rotate_matrix.inverse(); pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix);// 这边为什么要将 首个激光点 旋转到 雷达的X轴上去呢？ velocity_ = rotate_matrix * velocity_;// 没能明白您这里的线速度和角速度为什么还需要坐标变换，您能解释下吗，麻烦了。 angular_rate_ = rotate_matrix * angular_rate_; for (size_t point_index = 1; point_index < origin_cloud_ptr-&gt;points.size(); ++point_index) { float orientation = atan2(origin_cloud_ptr-&gt;points[point_index].y, origin_cloud_ptr-&gt;points[point_index].x); if (orientation < 0.0) orientation += 2.0 * M_PI; if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space) continue; float real_time = fabs(orientation) / orientation_space * scan_period_ - scan_period_ / 2.0; Eigen::Vector3f origin_point(origin_cloud_ptr-&gt;points[point_index].x, origin_cloud_ptr-&gt;points[point_index].y, origin_cloud_ptr-&gt;points[point_index].z); Eigen::Matrix3f current_matrix = UpdateMatrix(real_time); Eigen::Vector3f rotated_point = current_matrix * origin_point; Eigen::Vector3f adjusted_point = rotated_point + velocity_ * real_time; CloudData::POINT point; point.x = adjusted_point(0); point.y = adjusted_point(1); point.z = adjusted_point(2); output_cloud_ptr-&gt;points.push_back(point); } pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, transform_matrix.inverse()); return true; } — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub, or unsubscribe.

[LeegoChen]

大概明白您的意思了，您看我理解有没有错。 但这样可能会带来一个问题。假如当中间某个点求出来的角度是1°，没有halfpassed，也就没法知道这个点到底是1°还是361°，也就没法判断这个点相对swap第一个点，雷达是转了1°还是361°。 所以您通过下面的判断，不对第一个点附近的点做处理。
if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space) continue;
微信已加，望能多向您学习！

[luameows]

@FrogyChen 请教下关于第2点中，gnss_pose_ *= lidar_to_imu_;这里的乘法顺序是不是写反了，位姿变换应该是左乘lidar_to_imu_的方式，将坐标系转换到velo_link下吧