关于坐标系的问题

[designershell]

我用自己采集的数据生成pcd，双激光雷达位于车体的右前方和左后方，我想将双激光点云均转换到车体坐标系base_link下，然后再进行处理；可是尽管VIEWPOINT的位姿是正确的，点云也正常经过了VIEWPOINT位姿的变换，但是，我发现动态点的分离出现了大量错误，我需要如何修改代码才能正常处理这种情况？

[Kin-Zhang]

从你的描述中，有以下点需要注意：

1. 两个雷达，那就应该是两个lidar中心，现在PCD的这个方式只能保留一个viewpoint
2. 如果是两个lidar直接 拼到base_link，需要特别注意拼的对不对
3. 不是修改dufomap代码，要修改数据处理代码，处理方式可能就不能直接用pcd了；我之前也有过多个lidar的数据要处理，你可以看一下这个pull request的记录（里面有一些我demo 起来检查的东西） Add MAN TruckScenes data processing into codebase OpenSceneFlow#19 (comment)

如果有问题的话 可能还是需要你上传原始数据，你处理文件的流程；然后我再修改

[AutopilotTech]

从你的描述中，有以下点需要注意：

1. 两个雷达，那就应该是两个lidar中心，现在PCD的这个方式只能保留一个viewpoint

2. 如果是两个lidar直接 拼到base_link，需要特别注意拼的对不对

3. 不是修改dufomap代码，要修改数据处理代码，处理方式可能就不能直接用pcd了；我之前也有过多个lidar的数据要处理，你可以看一下这个pull request的记录（里面有一些我demo 起来检查的东西） [Add MAN TruckScenes data processing into codebase OpenSceneFlow#19 (comment)](https://github.com/KTH-RPL/OpenSceneFlow/pull/19#issuecomment-3326974552)

如果有问题的话 可能还是需要你上传原始数据，你处理文件的流程；然后我再修改

右前激光，在sensor视角下直接处理看起来正常
https://drive.google.com/file/d/1Tm651JoVYywwcLqlhUxpd6-I62K0gU1j/view?usp=sharing
左后激光，在sensor视角下直接处理看起来也正常
https://drive.google.com/file/d/1J9F0vKNenrSanEbIF2bp8GNVcPL9b_0t/view?usp=sharing
合并双激光，点云变换到车体base_link坐标系，pcd中的VIEWPOINT是base_link到world的变换，此时看起来不太正常
https://drive.google.com/file/d/1g2FMKtYEeSnjOGWXZiRj5iG3Y-VuWfqV/view?usp=sharing

[designershell]

@Kin-Zhang ，为了避免时间同步，外参标定等误差影响，对每个lidar单独处理后再做融合，可能这样的处理流程更好？

[Kin-Zhang]

sorry 最近还在开题报告准备；还没来得及下载数据

我可以先解释一下：

1. dufomap 主要是依靠lidar的点的ray-casting path 建立一个freespace map
2. 那么当有多个lidar的时候 这个ray-casting 应该是以每个lidar自身的pose去做
3. 这个流程下 两个lidar 时间同步其实没有太多所谓，只要pose是准的即可（有误差其实也能用 大概10-20cm 都能）

所以在pull request里，我主要是先把两个lidar 放到base_link下，然后再每个lidar根据自身的center 位置做的ray-casting，map原点是base-link（车后轴）；所以 你提供的信息里 应该还需要告诉我两个lidar的外参？

字有点多 希望我解释清楚了

对应参考 short 代码：

https://github.com/KTH-RPL/OpenSceneFlow/blob/c5f5746d529743c90adf8a33334e2a4e8792f1cf/process.py#L246-L270

[designershell]

sorry 最近还在开题报告准备；还没来得及下载数据

我可以先解释一下：

1. dufomap 主要是依靠lidar的点的ray-casting path 建立一个freespace map
2. 那么当有多个lidar的时候 这个ray-casting 应该是以每个lidar自身的pose去做
3. 这个流程下 两个lidar 时间同步其实没有太多所谓，只要pose是准的即可（有误差其实也能用 大概10-20cm 都能）

所以在pull request里，我主要是先把两个lidar 放到base_link下，然后再每个lidar根据自身的center 位置做的ray-casting，map原点是base-link（车后轴）；所以 你提供的信息里 应该还需要告诉我两个lidar的外参？

字有点多 希望我解释清楚了

对应参考 short 代码：

https://github.com/KTH-RPL/OpenSceneFlow/blob/c5f5746d529743c90adf8a33334e2a4e8792f1cf/process.py#L246-L270

感谢您能抽时间答复
front_rslidar 相对base_link外参
"0.883", "2.694", "1.645", # x(right), y(forward), z(up) translation
"1.545", "0.03", "3.11", # yaw, pitch, roll (radians)
rear_rslidar相对base_link外参
"-0.883", "-0.409", "1.645", # x(right), y(forward), z(up) translation
"-1.575", "0.01", "3.12", # yaw, pitch, roll (radians)

[Kin-Zhang]

谢谢提供数据和这些信息，我得空下载了数据并查看了，你的前两个数据

右前激光，在sensor视角下直接处理看起来正常
https://drive.google.com/file/d/1Tm651JoVYywwcLqlhUxpd6-I62K0gU1j/view?usp=sharing
左后激光，在sensor视角下直接处理看起来也正常
https://drive.google.com/file/d/1J9F0vKNenrSanEbIF2bp8GNVcPL9b_0t/view?usp=sharing

然后直接看了一下这两个数据 在point 直接加起来的map是对的，而且还比我想的处理的更好一点（虽然没看到你的process file哈）

1. 两个lidar的pose 都是以自身lidar作为axis，这一点从图1 可以看出来；两个axis都是 发射点
2. 两个lidar的pose 都已经和ego对齐了，这一点我直接 把两个lidar的pcd加起来能直接得到正确的map可知 （见图2 左图raw map是我直接pcd里的文件相加得到的）

所以处理流程我猜测是，直接得到1）ego2world pose后，再根据2）lidar2ego 进行pcd的处理（处理方式挺好的 省去很多麻烦）

这个帖子最后是我用来正确run-multi-lidar的dufomap file （python版哈，直接py>=3.8, pip install dufomap就能运行），这是我运行的结果：

Multi-lidar dufomap clean up demo python-code. Click me to expand

"""
# Created: 2024-11-20 13:11
# Copyright (C) 2024-now, RPL, KTH Royal Institute of Technology
# Author: Qingwen Zhang  (https://kin-zhang.github.io/)
#
# This file is part of DUFOMap (https://github.com/KTH-RPL/dufomap) and 
# DynamicMap Benchmark (https://github.com/KTH-RPL/DynamicMap_Benchmark) projects.
# If you find this repo helpful, please cite the respective publication as 
# listed on the above website.

# Description: Output Cleaned Map through Python API.
"""
from pathlib import Path
import os, fire, time
import numpy as np
from tqdm import tqdm

from dufomap import dufomap
from dufomap.utils import pcdpy3
def inv_pose_matrix(pose):
    inv_pose = np.eye(4)
    inv_pose[:3, :3] = pose[:3, :3].T
    inv_pose[:3, 3] = -pose[:3, :3].T.dot(pose[:3, 3])
    return inv_pose

MIN_AXIS_RANGE = 0.2 # HARD CODED: remove ego vehicle points
MAX_AXIS_RANGE = 50 # HARD CODED: remove far away points

class DynamicMapData:
    def __init__(self, directory):
        self.scene_id = directory.split("/")[-1]
        self.directory = Path(directory) / "pcd"
        self.pcd_files = [os.path.join(self.directory, f) for f in sorted(os.listdir(self.directory)) if f.endswith('.pcd')]

    def __len__(self):
        return len(self.pcd_files)
    
    def __getitem__(self, index_):
        res_dict = {
            'scene_id': self.scene_id,
            'timestamp': self.pcd_files[index_].split("/")[-1].split(".")[0],
        }
        pcd_ = pcdpy3.PointCloud.from_path(self.pcd_files[index_])
        pc0 = pcd_.np_data[:,:3]
        res_dict['pc'] = pc0.astype(np.float32)
        res_dict['pose'] = list(pcd_.viewpoint)
        return res_dict

def main_vis(
    data_dir: list = [
        "/home/kin/DATA_HDD/issue_data/dufomap/33/front_rslidar", 
        "/home/kin/DATA_HDD/issue_data/dufomap/33/rear_rslidar"
    ],
    voxel_map: bool = False,
):
    # STEP 0: Load all lidar data
    lidar_data = dict()
    lidar_names = []
    for dir_ in data_dir:
        name = dir_.split("/")[-1]
        lidar_names.append(name)
        lidar_data[name] = DynamicMapData(dir_)
        print(f"Loaded {len(lidar_data[name])} frames from {dir_}")
    
    # STEP 0: initialize 
    mydufo = dufomap(0.1, 0.2, 2, num_threads=12) # resolution, d_s, d_p same with paper.
    cloud_acc = np.zeros((0, 3), dtype=np.float32)

    total_frames = sum(len(lidar_data[name]) for name in lidar_names)
    pbar = tqdm(total=total_frames, ncols=100)
    for lid, name in enumerate(lidar_names):
        for frame_id in range(len(lidar_data[name])):
            data = lidar_data[name][frame_id]
            pbar.set_description(f"Lidar: {name}, Frame: {frame_id}, ts: {data['timestamp']}")
            norm_pc0 = np.linalg.norm(data['pc'][:, :3] - data['pose'][:3], axis=1)
            range_mask = (
                    (norm_pc0>MIN_AXIS_RANGE) & 
                    (norm_pc0<MAX_AXIS_RANGE)
            )
            # STEP 1: integrate point cloud into dufomap
            mydufo.run(data['pc'][range_mask], data['pose'], cloud_transform = False)
            # STEP 1: integrate point cloud into dufomap
            cloud_acc = np.concatenate((cloud_acc, data['pc']), axis=0)
            pbar.update(1)

    # STEP 2: propagate
    mydufo.oncePropagateCluster(if_propagate=True, if_cluster=False)
    # STEP 3: Map results; You can save the voxel map directly based on the resolution we set before:
    mydufo.outputMap(cloud_acc, voxel_map=voxel_map)
    # save cloud_acc as pcd, debug purpose
    # pcdpy3.save_pcd("accumulated_cloud.pcd", cloud_acc)
    mydufo.printDetailTiming()

if __name__ == "__main__":
    start_time = time.time()
    fire.Fire(main_vis)
    print(f"Time used: {time.time() - start_time:.2f} s")

最后想问一下 同学介意把你那边的数据处理code （如果简单的话）和这个数据 作为演示数据（会提及你的账号为multi-lidar demo提供者）吗？我可能后面更新一下大家怎么 1）处理多雷达；2）运行dufomap的这套流程

[designershell]

非常感谢您的回答，我是按照您提供的Teaser data (KITTI 00: 384.4Mb) 格式保存的pcd，并对每个sensor下的点云进行了sensor_to_world的变换，VIEWPOINT为sensor_to_world位姿；您可以将这个数据作为演示数据。
// 保存点云为二进制 PCD，字段为 x y z intensity
void SavePointCloudAsBinaryPCD(const pcl::PointCloudpcl::PointXYZI::Ptr& cloud,
const std::string& filename, const Rigid3d& viewpoint) {
std::ofstream ofs;
ofs.exceptions(std::ofstream::badbit | std::ofstream::failbit);
try {
ofs.open(filename, std::ios::out | std::ios::binary);
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "Failed to open PCD file for writing: %s (%s)", filename.c_str(), e.what());
return;
}

// Header
ofs << "# .PCD v0.7 - Point Cloud Data file format\n";
ofs << "VERSION 0.7\n";
ofs << "FIELDS x y z intensity\n";
ofs << "SIZE 4 4 4 4\n";
ofs << "TYPE F F F F\n";
ofs << "COUNT 1 1 1 1\n";
ofs << "WIDTH " << cloud->size() << "\n";
ofs << "HEIGHT 1\n";

// VIEWPOINT: tx ty tz qw qx qy qz
double tx = viewpoint.translation().x();
double ty = viewpoint.translation().y();
double tz = viewpoint.translation().z();
double qw = viewpoint.rotation().w();
double qx = viewpoint.rotation().x();
double qy = viewpoint.rotation().y();
double qz = viewpoint.rotation().z();
ofs << std::fixed << std::setprecision(6);
ofs << "VIEWPOINT " << tx << ' ' << ty << ' ' << tz << ' ' << qw << ' ' << qx << ' ' << qy << ' ' << qz << "\n";

ofs << "POINTS " << cloud->size() << "\n";
ofs << "DATA binary\n";

// 写入二进制数据
std::vector buffer;
buffer.reserve(cloud->size() * 4);
for (const auto& p : cloud) {
Eigen::Vector3d pt(p.x, p.y, p.z);
Eigen::Vector3d pt_odom = viewpoint * pt;
buffer.push_back(static_cast(pt_odom.x()));
buffer.push_back(static_cast(pt_odom.y()));
buffer.push_back(static_cast(pt_odom.z()));
buffer.push_back(static_cast(p.intensity));
}
ofs.write(reinterpret_cast<const char>(buffer.data()), buffer.size() * sizeof(float));

ofs.close();
RCLCPP_DEBUG(this->get_logger(), "Saved PCD: %s (points=%zu)", filename.c_str(), cloud->size());
}

[github-actions]

This issue is stale because it has been open for 30 days with no activity.
It will be closed if no further activity occurs. Let us know if you still need help!

[github-actions]

This issue is being closed because it has been stale for 14 days with no activity.
If you still need help, please feel free to leave comments.