LimHyungTae

def R2ypr(R: np.ndarray, unit="rad"):
    n = R[:, 0]
    o = R[:, 1]
    a = R[:, 2]

    y = np.arctan2(n[1], n[0])
    p = np.arctan2(-n[2], n[0] * np.cos(y) + n[1] * np.sin(y))
    r = np.arctan2(a[0] * np.sin(y) - a[1] * np.cos(y),
                   -o[0] * np.sin(y) + o[1] * np.cos(y))
    

LimHyungTae

def plot_traj_open3d(ref: PoseTrajectory3D, est: PoseTrajectory3D, axis_len=0.2, skip=5):
    geometries = []

    # Reference trajectory (blue frames)
    for i, T in enumerate(ref.poses_se3):
        if i % skip != 0:
            continue
        mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=axis_len)
        mesh.transform(T)
        geometries.append(mesh)

LimHyungTae

import open3d as o3d
import os
import time

# PCD 파일이 저장된 폴더 경로
pcd_folder = "./"

# PCD 파일 목록 가져오기
pcd_files = sorted([f for f in os.listdir(pcd_folder) if f.endswith(".pcd")])

LimHyungTae

import ray
import torch
import os
import torch
print("CUDA Available:", torch.cuda.is_available())
print("CUDA Device Count:", torch.cuda.device_count())
print("Current CUDA Device:", torch.cuda.current_device())
print("CUDA Device Name:", torch.cuda.get_device_name(torch.cuda.current_device()))

print(os.environ.get("CUDA_VISIBLE_DEVICES"))

LimHyungTae

#include <iostream>
#include <string>
#include <vector>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <tools/nvutils.h>

#include "tools/pose_utils.hpp"

LimHyungTae

#!/bin/bash

# Find all compile_commands.json files under ./build directory
find ./build -type f -name "compile_commands.json" | while read src_file; do
    # Extract the package name from the file path
    package_name=$(echo "$src_file" | sed -e 's|./build/\([^/]*\)/compile_commands.json|\1|')
    
    # Define the destination path
    dest_file="./src/${package_name}/compile_commands.json"
    

LimHyungTae

import numpy as np

rot = np.array([[9.999976e-01, 7.553071e-04, -2.035826e-03], [-7.854027e-04, 9.998898e-01, -1.482298e-02], [2.024406e-03, 1.482454e-02, 9.998881e-01]])
t = np.array([-8.086759e-01, 3.195559e-01, -7.997231e-01])

tf = np.eye(4)

tf[:3, :3] = rot
tf[:3, 3] = t

LimHyungTae

#include <iostream>
#include <vector>
#include <utility>
#include <queue>

using namespace std;

void bfs(int start, vector<vector<int>>& adj, vector<bool>& visited, vector<int>& cluster) {
    queue<int> q;
    q.push(start);

LimHyungTae

#include "ceres/ceres.h"
#include "glog/logging.h"

using namespace std;

void PassByAddress(const Eigen::Vector3d *b) {
    cout << (*b)(0) << ", " << (*b)(1) << ", " << (*b)(2) << endl;
}

void PassByData(const double* b) {

LimHyungTae

#include <iostream>
#include <cmath>

using namespace std;

double prob2logOdds(double prob) {
    return 2 * atanh(2 * prob - 1);
}

double logOdds2prob(double log_odds) {