Missing tool to calibrate lidar-lidar with SLAM + cloudcompare
The idea is two export two point clouds from two slams (on on each lidar), then find the transform to match one point cloud on the other in CloudCompare (for example).
code to start implementing the conversion of transform in CloudCompare + slam trajectories interpolators to the calibration transform:
` vtkNew l0; auto interp0 = vtkSmartPointer::New(); l0->SetInterpreter(interp0); l0->SetFileName("/baymax/data/Veoneer/data/derived/pcap/mytrace_00002_20190920144245.pcap.lidar0.pcap"); l0->SetCalibrationFileName("/home/gabriel/dev/LidarView/share/VLP-32c.xml"); l0->Update();
vtkNew l1; auto interp1 = vtkSmartPointer::New(); l1->SetInterpreter(interp1); l1->SetFileName("/baymax/data/Veoneer/data/derived/pcap/mytrace_00002_20190920144245.pcap.lidar2.pcap"); l1->SetCalibrationFileName("/home/gabriel/dev/LidarView/share/VLP-16.xml"); l1->Update(); std::cout << l0->GetNetworkTimeToDataTime() - l1->GetNetworkTimeToDataTime() << std::endl; std::cout << l0->GetNetworkTimeToDataTime() << std::endl;
auto traj0_ = vtkTemporalTransformsReader::OpenTemporalTransforms("/baymax/data/Veoneer/data/derived/pcap/mytrace_00002_20190920144245.pcap-trajectory.poses"); auto traj1_ = vtkTemporalTransformsReader::OpenTemporalTransforms("/baymax/data/Veoneer/data/derived/pcap/mytrace_00002_20190920144245.pcap-lidar-2-trajectory.poses"); auto traj0 = traj0_->CreateInterpolator(); traj0->SetInterpolationTypeToLinear(); auto traj1 = traj1_->CreateInterpolator(); traj1->SetInterpolationTypeToLinear();
double t_network = 1568958276.37; double t = t_network + l0->GetNetworkTimeToDataTime();
// Eigen::Matrix3d R_l1_l0_w; // L1_L0 // R_l1_l0_w << 0.78, 0.188, 0.588, // -0.099, 0.979, -0.180, // -0.609, 0.083, 0.789; // Eigen::Vector3d T_l1_l0_w; // T_l1_l0_w << -29.467, 21.061, -9.552;
// Eigen::Matrix3d R_l1_l0_w; // L1_L0, 2nd attempt // R_l1_l0_w << .789, 0.185, 0.586, // -0.096, 0.979, -0.179, // -0.607, 0.085, 0.790; // Eigen::Vector3d T_l1_l0_w; // T_l1_l0_w << -28.421, 21.033, -9.803;
Eigen::Matrix3d R_l1_l0_w; // L2_L0 R_l1_l0_w << 0.770735, 0.112035,-0.627229, -0.150847, 0.988518, -0.008792, 0.619042, 0.101392, 0.778785;
Eigen::Vector3d T_l1_l0_w; T_l1_l0_w << -13.632252, 36.124470, -7.686131;
Eigen::Transform<double, 3, Eigen::Isometry> l1_l0_w = ToEigenTransform(R_l1_l0_w, T_l1_l0_w);
Eigen::Transform<double, 3, Eigen::Isometry> l1_w = Interpolate(traj1, t); Eigen::Transform<double, 3, Eigen::Isometry> l0_w = Interpolate(traj0, t);
// std::cout << l1_l0_w.rotation() << ", " << std::endl<< l1_l0_w.translation() << std::endl; Eigen::Transform<double, 3, Eigen::Isometry> l1_to_l0 = l0_w.inverse() * l1_l0_w * l1_w; std::cout << l1_to_l0.rotation() << "(" << (180.0 / vtkMath::Pi()) * MatrixToRollPitchYaw(l1_to_l0.rotation()) << ")" << std::endl<< l1_to_l0.translation() << std::endl; `
migrated from: https://gitlab.kitware.io/ComputerVision/LidarView/issues/26