hofee/nbv_rec_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
hofee 2024-10-10 21:48:55 +08:00
parent 8fd2d6b1e1
commit cd85fed3a0
10 changed files with 73 additions and 33004 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
cam_directions.txt
View File

@ -1,160 +0,0 @@
-2.976360870451021934e-01 2.612396282015549270e-02 2.722307168556427626e-01
-2.807145935740879561e-01 2.344288301708187527e-02 2.619120507693710187e-01
-2.637931001030737743e-01 2.076180321400825785e-02 2.515933846830992748e-01
-2.468716066320595925e-01 1.808072341093463695e-02 2.412747185968275587e-01
-2.299501131610453553e-01 1.539964360786101778e-02 2.309560525105558149e-01
-2.130286196900311735e-01 1.271856380478740209e-02 2.206373864242840988e-01
-1.961071262190169640e-01 1.003748400171378119e-02 2.103187203380123549e-01
-1.791856327480027822e-01 7.356404198640163761e-03 2.000000542517406110e-01
-1.622641392769885726e-01 4.675324395566546332e-03 1.896813881654688672e-01
-1.453426458059743631e-01 1.994244592492925433e-03 1.793627220791971233e-01
1.058318675876641773e-01 1.913315480742055485e-01 3.701966140390944293e-01
9.931276171077207948e-02 1.796797252938450440e-01 3.553058019571109782e-01
9.279365583387996774e-02 1.680279025134845672e-01 3.404149898751275827e-01
8.627454995698786988e-02 1.563760797331240904e-01 3.255241777931441316e-01
7.975544408009577202e-02 1.447242569527635858e-01 3.106333657111607360e-01
7.323633820320367416e-02 1.330724341724031090e-01 2.957425536291773405e-01
6.671723232631157630e-02 1.214206113920426183e-01 2.808517415471938894e-01
6.019812644941947150e-02 1.097687886116821415e-01 2.659609294652104938e-01
5.367902057252737363e-02 9.811696583132166471e-02 2.510701173832270983e-01
4.715991469563526883e-02 8.646514305096116015e-02 2.361793053012436749e-01
-3.921455052209112946e-01 5.125427963019697081e-02 2.092982359830542760e-01
-3.736810423058752328e-01 4.853268829412461099e-02 2.021109868668294895e-01
-3.552165793908391711e-01 4.581109695805224424e-02 1.949237377506047031e-01
-3.367521164758031094e-01 4.308950562197988443e-02 1.877364886343799166e-01
-3.182876535607670476e-01 4.036791428590751768e-02 1.805492395181551302e-01
-2.998231906457310414e-01 3.764632294983515093e-02 1.733619904019303437e-01
-2.813587277306949241e-01 3.492473161376279112e-02 1.661747412857055572e-01
-2.628942648156589179e-01 3.220314027769043130e-02 1.589874921694807708e-01
-2.444298019006228562e-01 2.948154894161806455e-02 1.518002430532560121e-01
-2.259653389855867667e-01 2.675995760554569780e-02 1.446129939370311979e-01
2.966121428882431688e-01 -1.243668772355264462e-01 2.528267403420925707e-01
2.794781650275093843e-01 -1.175232445334360720e-01 2.451071876279281303e-01
2.623441871667755443e-01 -1.106796118313456839e-01 2.373876349137637176e-01
2.452102093060417598e-01 -1.038359791292553097e-01 2.296680821995992772e-01
2.280762314453079753e-01 -9.699234642716492161e-02 2.219485294854348645e-01
2.109422535845741908e-01 -9.014871372507454739e-02 2.142289767712704240e-01
1.938082757238404064e-01 -8.330508102298417317e-02 2.065094240571059836e-01
1.766742978631065941e-01 -7.646144832089379895e-02 1.987898713429415709e-01
1.595403200023728096e-01 -6.961781561880342473e-02 1.910703186287771582e-01
1.424063421416389974e-01 -6.277418291671303663e-02 1.833507659146127178e-01
1.516386400982264460e-01 -1.586701464075628287e-01 2.415754089606511334e-01
1.434280702011365705e-01 -1.455541040346652326e-01 2.289043405590775460e-01
1.352175003040466672e-01 -1.324380616617676643e-01 2.162332721575039862e-01
1.270069304069567917e-01 -1.193220192888700681e-01 2.035622037559303987e-01
1.187963605098669162e-01 -1.062059769159724720e-01 1.908911353543568112e-01
1.105857906127770407e-01 -9.308993454307488979e-02 1.782200669527832515e-01
1.023752207156871513e-01 -7.997389217017729368e-02 1.655489985512096363e-01
9.416465081859727582e-02 -6.685784979727971145e-02 1.528779301496360765e-01
8.595408092150740031e-02 -5.374180742438212921e-02 1.402068617480624890e-01
7.774351102441751094e-02 -4.062576505148451922e-02 1.275357933464889015e-01
-1.305310885675468879e-01 -2.574515835723965029e-01 3.302868348637944540e-01
-1.245164210467534782e-01 -2.462217366382081774e-01 3.148688482102455222e-01
-1.185017535259600685e-01 -2.349918897040197963e-01 2.994508615566965903e-01
-1.124870860051666588e-01 -2.237620427698314429e-01 2.840328749031476585e-01
-1.064724184843732491e-01 -2.125321958356430896e-01 2.686148882495986712e-01
-1.004577509635798394e-01 -2.013023489014547363e-01 2.531969015960497393e-01
-9.444308344278641576e-02 -1.900725019672663829e-01 2.377789149425008075e-01
-8.842841592199301992e-02 -1.788426550330780018e-01 2.223609282889518757e-01
-8.241374840119961021e-02 -1.676128080988896762e-01 2.069429416354029438e-01
-7.639908088040618661e-02 -1.563829611647012952e-01 1.915249549818539843e-01
-2.141758616454684239e-01 -1.401142265731682157e-01 3.208142574142802683e-01
-2.034685215614439324e-01 -1.329408282550643694e-01 3.055206239383900790e-01
-1.927611814774194687e-01 -1.257674299369604953e-01 2.902269904624999453e-01
-1.820538413933950050e-01 -1.185940316188566629e-01 2.749333569866098115e-01
-1.713465013093705136e-01 -1.114206333007528027e-01 2.596397235107196777e-01
-1.606391612253460499e-01 -1.042472349826489564e-01 2.443460900348295162e-01
-1.499318211413215862e-01 -9.707383666454511006e-02 2.290524565589393546e-01
-1.392244810572971225e-01 -8.990043834644126375e-02 2.137588230830492209e-01
-1.285171409732726311e-01 -8.272704002833740355e-02 1.984651896071590871e-01
-1.178098008892481535e-01 -7.555364171023355724e-02 1.831715561312689255e-01
-1.002417282812653465e-01 -1.857441582228861743e-01 2.313739119119407606e-01
-9.359755865267997688e-02 -1.758149544613172577e-01 2.153344006161490576e-01
-8.695338902409460724e-02 -1.658857506997483688e-01 1.992948893203573268e-01
-8.030921939550925148e-02 -1.559565469381794800e-01 1.832553780245656239e-01
-7.366504976692386797e-02 -1.460273431766105634e-01 1.672158667287739209e-01
-6.702088013833851221e-02 -1.360981394150416746e-01 1.511763554329821901e-01
-6.037671050975313564e-02 -1.261689356534727580e-01 1.351368441371904594e-01
-5.373254088116777294e-02 -1.162397318919038830e-01 1.190973328413987842e-01
-4.708837125258240330e-02 -1.063105281303349803e-01 1.030578215456070812e-01
-4.044420162399703367e-02 -9.638132436876607756e-02 8.701831024981535045e-02
1.554821963443976385e-01 -2.148257011199891098e-01 5.250742981755206484e-01
1.482802959028949041e-01 -2.064840275062017061e-01 5.083845075431692173e-01
1.410783954613921698e-01 -1.981423538924143024e-01 4.916947169108177862e-01
1.338764950198894355e-01 -1.898006802786268987e-01 4.750049262784663551e-01
1.266745945783867011e-01 -1.814590066648394673e-01 4.583151356461149240e-01
1.194726941368839807e-01 -1.731173330510520636e-01 4.416253450137634928e-01
1.122707936953812463e-01 -1.647756594372646599e-01 4.249355543814120062e-01
1.050688932538785120e-01 -1.564339858234772562e-01 4.082457637490606306e-01
9.786699281237579151e-02 -1.480923122096898525e-01 3.915559731167091995e-01
9.066509237087304329e-02 -1.397506385959024211e-01 3.748661824843577128e-01
8.025776028547672303e-02 2.944407235099611442e-01 2.936572800346861634e-01
7.539005914741314651e-02 2.795810656774440073e-01 2.811874743132241616e-01
7.052235800934955612e-02 2.647214078449268704e-01 2.687176685917621599e-01
6.565465687128596572e-02 2.498617500124097057e-01 2.562478628703001582e-01
6.078695573322238921e-02 2.350020921798925411e-01 2.437780571488381842e-01
5.591925459515880575e-02 2.201424343473754042e-01 2.313082514273761825e-01
5.105155345709521536e-02 2.052827765148582673e-01 2.188384457059141808e-01
4.618385231903163191e-02 1.904231186823411304e-01 2.063686399844521790e-01
4.131615118096804845e-02 1.755634608498239935e-01 1.938988342629902051e-01
3.644845004290446500e-02 1.607038030173068288e-01 1.814290285415282034e-01
-5.157790449341418532e-02 -2.280915238559273472e-01 2.331722629466718155e-01
-4.687897960620368565e-02 -2.143414166823043310e-01 2.194298229297667047e-01
-4.218005471899319292e-02 -2.005913095086813147e-01 2.056873829128615938e-01
-3.748112983178270019e-02 -1.868412023350582984e-01 1.919449428959564830e-01
-3.278220494457220052e-02 -1.730910951614352822e-01 1.782025028790513721e-01
-2.808328005736170779e-02 -1.593409879878122659e-01 1.644600628621462890e-01
-2.338435517015120813e-02 -1.455908808141892496e-01 1.507176228452411781e-01
-1.868543028294071540e-02 -1.318407736405662334e-01 1.369751828283360673e-01
-1.398650539573022267e-02 -1.180906664669432449e-01 1.232327428114309703e-01
-9.287580508519722999e-03 -1.043405592933202286e-01 1.094903027945258456e-01
-1.269215451024525432e-01 2.625545655654951682e-01 3.417737734235570812e-01
-1.199871749412033256e-01 2.471706458737163714e-01 3.310383402518738971e-01
-1.130528047799540942e-01 2.317867261819375746e-01 3.203029070801906575e-01
-1.061184346187048627e-01 2.164028064901587500e-01 3.095674739085074179e-01
-9.918406445745564515e-02 2.010188867983799255e-01 2.988320407368242337e-01
-9.224969429620641370e-02 1.856349671066011287e-01 2.880966075651409941e-01
-8.531532413495718226e-02 1.702510474148223041e-01 2.773611743934577545e-01
-7.838095397370795081e-02 1.548671277230435073e-01 2.666257412217745704e-01
-7.144658381245873324e-02 1.394832080312647105e-01 2.558903080500913307e-01
-6.451221365120950180e-02 1.240992883394858581e-01 2.451548748784081189e-01
-2.341853875412766850e-01 5.152039859854373044e-02 5.197893180168721150e-01
-2.252007148693486449e-01 4.944684319447055498e-02 5.020417541005327555e-01
-2.162160421974206326e-01 4.737328779039738647e-02 4.842941901841934516e-01
-2.072313695254926202e-01 4.529973238632421795e-02 4.665466262678541476e-01
-1.982466968535646079e-01 4.322617698225104943e-02 4.487990623515148436e-01
-1.892620241816365678e-01 4.115262157817788091e-02 4.310514984351755396e-01
-1.802773515097085555e-01 3.907906617410470546e-02 4.133039345188362357e-01
-1.712926788377805432e-01 3.700551077003153000e-02 3.955563706024969317e-01
-1.623080061658525031e-01 3.493195536595836148e-02 3.778088066861576833e-01
-1.533233334939244907e-01 3.285839996188519296e-02 3.600612427698183238e-01
1.210235277878131122e-01 -3.921239465826816817e-01 2.952365626755736328e-01
1.155743475904795203e-01 -3.762006607063697050e-01 2.844312213076097273e-01
1.101251673931459424e-01 -3.602773748300577838e-01 2.736258799396458774e-01
1.046759871958123506e-01 -3.443540889537458072e-01 2.628205385716819720e-01
9.922680699847875874e-02 -3.284308030774338305e-01 2.520151972037181221e-01
9.377762680114518079e-02 -3.125075172011218538e-01 2.412098558357542721e-01
8.832844660381158897e-02 -2.965842313248098772e-01 2.304045144677903667e-01
8.287926640647799714e-02 -2.806609454484979560e-01 2.195991730998265168e-01
7.743008620914440532e-02 -2.647376595721859793e-01 2.087938317318626391e-01
7.198090601181081349e-02 -2.488143736958740027e-01 1.979884903638987614e-01
-7.634513431648837223e-02 1.841063024461145892e-01 3.127523209464129761e-01
-7.178553403421568391e-02 1.753640395784209771e-01 2.953516633376314648e-01
-6.722593375194299559e-02 1.666217767107273651e-01 2.779510057288500091e-01
-6.266633346967032114e-02 1.578795138430337808e-01 2.605503481200685534e-01
-5.810673318739763282e-02 1.491372509753401687e-01 2.431496905112870421e-01
-5.354713290512495838e-02 1.403949881076465844e-01 2.257490329025055864e-01
-4.898753262285227006e-02 1.316527252399529724e-01 2.083483752937241029e-01
-4.442793234057959562e-02 1.229104623722593881e-01 1.909477176849426472e-01
-3.986833205830691423e-02 1.141681995045657899e-01 1.735470600761611637e-01
-3.530873177603422591e-02 1.054259366368721779e-01 1.561464024673796802e-01
-2.097103305076620516e-01 -1.384789009505066615e-01 2.811056276500599749e-01
-1.943908481741861705e-01 -1.318322337484758855e-01 2.700994804522541815e-01
-1.790713658407102893e-01 -1.251855665464451373e-01 2.590933332544483880e-01
-1.637518835072344081e-01 -1.185388993444143890e-01 2.480871860566425391e-01
-1.484324011737584992e-01 -1.118922321423836130e-01 2.370810388588367179e-01
-1.331129188402826458e-01 -1.052455649403528648e-01 2.260748916610308967e-01
-1.177934365068067368e-01 -9.859889773832210269e-02 2.150687444632250478e-01
-1.024739541733308557e-01 -9.195223053629135446e-02 2.040625972654192544e-01
-8.715447183985497448e-02 -8.530556333426059235e-02 1.930564500676134332e-01
-7.183498950637906555e-02 -7.865889613222983023e-02 1.820503028698076120e-01

16
cam_poses.txt
View File

@ -1,16 +0,0 @@
-3.145575805161163752e-01 2.880504262322911013e-02 2.825493829419145064e-01
1.123509734645562752e-01 2.029833708545660254e-01 3.850874261210778249e-01
-4.106099681359473563e-01 5.397587096626933756e-02 2.164854850992790625e-01
3.137461207489769532e-01 -1.312105099376168205e-01 2.605462930562569834e-01
1.598492099953163215e-01 -1.717861887804604248e-01 2.542464773622247209e-01
-1.365457560883402977e-01 -2.686814305065848840e-01 3.457048215173433858e-01
-2.248832017294928876e-01 -1.472876248912720620e-01 3.361078908901704021e-01
-1.068858979098507161e-01 -1.956733619844550631e-01 2.474134232077324635e-01
1.626840967859003728e-01 -2.231673747337765135e-01 5.417640888078720796e-01
8.512546142354031342e-02 3.093003813424782811e-01 3.061270857561481651e-01
-5.627682938062467805e-02 -2.418416310295503635e-01 2.469147029635769264e-01
-1.338559152637017746e-01 2.779384852572739928e-01 3.525092065952403209e-01
-2.431700602132046973e-01 5.359395400261689896e-02 5.375368819332113635e-01
1.264727079851467040e-01 -4.080472324589936584e-01 3.060419040435374827e-01
-8.090473459876104667e-02 1.928485653138081735e-01 3.301529785551944318e-01
-2.250298128411379328e-01 -1.451255681525374097e-01 2.921117748478658238e-01

2
configs/cad_config.yaml
View File

@ -15,7 +15,7 @@ runner:
model_dir: "/home/yan20/Desktop/nbv_rec/data/models"
table_model_path: "/home/yan20/Desktop/nbv_rec/data/table.obj"
model_start_idx: 0
voxel_size: 0.005
voxel_size: 0.002
max_view: 512
min_view: 128
max_diag: 0.7

32768
mesh_point_cloud.txt
View File

File diff suppressed because it is too large Load Diff

81
runners/cad_strategy.py
View File

@ -14,7 +14,7 @@ from utils.control_util import ControlUtil
from utils.communicate_util import CommunicateUtil
from utils.pts_util import PtsUtil
from utils.reconstruction_util import ReconstructionUtil
from utils.preprocess_util import save_scene_data
from utils.preprocess_util import save_scene_data, save_scene_data_multithread
from utils.data_load import DataLoadUtil
from utils.view_util import ViewUtil
@ -59,6 +59,8 @@ class CADStrategyRunner(Runner):
return scan_pts, obj_pts
def run_one_model(self, model_name):
result = dict()
shot_pts_list = []
ControlUtil.connect_robot()
''' init robot '''
@ -72,30 +74,26 @@ class CADStrategyRunner(Runner):
Log.info("[Part 1/5] take first view data")
view_data = CommunicateUtil.get_view_data(init=True)
first_cam_pts = ViewUtil.get_pts(view_data)
#shot_pts_list.append(first_cam_pts)
first_cam_to_real_world = ControlUtil.get_pose()
''' register '''
Log.info("[Part 1/5] do registeration")
cam_to_cad = PtsUtil.register(first_cam_pts, cad_model)
first_cam_to_world = ControlUtil.get_pose()
cad_to_world = first_cam_to_world @ np.linalg.inv(cam_to_cad)
cad_to_real_world = first_cam_to_real_world @ np.linalg.inv(cam_to_cad)
Log.success("[Part 1/5] finish init and register")
real_world_to_blender_world = np.eye(4)
real_world_to_blender_world[:3, 3] = np.asarray([0, 0, 0.9215])
cad_to_blender_world = real_world_to_blender_world @ cad_to_real_world
cad_model_blender_world:trimesh.Trimesh = cad_model.apply_transform(cad_to_blender_world)
world_to_blender_world = np.eye(4)
world_to_blender_world[:3, 3] = np.asarray([0, 0, 0.9215])
cad_to_blender_world = world_to_blender_world @ cad_to_world
temp_dir = "/home/yan20/nbv_rec/project/franka_control/temp_output"
cad_model:trimesh.Trimesh = cad_model.apply_transform(cad_to_blender_world)
cad_model.export(os.path.join(temp_dir, f"{temp_name}.obj"))
with tempfile.TemporaryDirectory() as temp_dir:
temp_dir = "/home/yan20/nbv_rec/project/franka_control/temp_output"
cad_model_blender_world.export(os.path.join(temp_dir, f"{temp_name}.obj"))
cad_model.export(os.path.join(temp_dir, f"{temp_name}.obj"))
scene_dir = os.path.join(temp_dir, temp_name)
''' sample view '''
Log.info("[Part 2/5] start running renderer")
result = subprocess.run([
subprocess.run([
self.blender_bin_path, '-b', '-P', self.generator_script_path, '--', temp_dir
], capture_output=True, text=True)
Log.success("[Part 2/5] finish running renderer")
@ -104,7 +102,7 @@ class CADStrategyRunner(Runner):
world_model_points = np.loadtxt(os.path.join(scene_dir, "points_and_normals.txt"))[:,:3]
''' preprocess '''
Log.info("[Part 3/5] start preprocessing data")
save_scene_data(temp_dir, temp_name)
save_scene_data_multithread(temp_dir, temp_name)
Log.success("[Part 3/5] finish preprocessing data")
pts_dir = os.path.join(temp_dir,temp_name,"pts")
@ -114,11 +112,11 @@ class CADStrategyRunner(Runner):
for frame_idx in range(frame_num):
pts_path = os.path.join(temp_dir,temp_name, "pts", f"{frame_idx}.txt")
idx_path = os.path.join(temp_dir,temp_name, "scan_points_indices", f"{frame_idx}.txt")
idx_path = os.path.join(temp_dir,temp_name, "scan_points_indices", f"{frame_idx}.npy")
point_cloud = np.loadtxt(pts_path)
if point_cloud.shape[0] != 0:
sampled_point_cloud = PtsUtil.voxel_downsample_point_cloud(point_cloud, self.voxel_size)
indices = np.loadtxt(idx_path, dtype=np.int32)
indices = np.load(idx_path)
try:
len(indices)
except:
@ -149,7 +147,7 @@ class CADStrategyRunner(Runner):
for idx, coverage_rate in limited_useful_view:
path = DataLoadUtil.get_path(temp_dir, temp_name, idx)
cam_info = DataLoadUtil.load_cam_info(path, binocular=True)
cam_to_world_seq.append(cam_info["cam_to_world"])
cam_to_world_seq.append(cam_info["cam_to_world_O"])
coveraget_rate_seq.append(coverage_rate)
idx_seq.append(idx)
render_pts.append(sample_view_pts_list[idx])
@ -157,32 +155,55 @@ class CADStrategyRunner(Runner):
Log.info("[Part 5/5] start running robot")
''' take best seq view '''
cad_model_real_world = cad_model_blender_world.apply_transform(np.linalg.inv(real_world_to_blender_world))
cad_model_real_world_pts = cad_model_real_world.vertices
cad_model_real_world.export(os.path.join(temp_dir, f"{temp_name}_real_world.obj"))
voxel_downsampled_cad_model_real_world_pts = PtsUtil.voxel_downsample_point_cloud(cad_model_real_world_pts, self.voxel_size)
result = dict()
gt_scanned_pts = np.concatenate(render_pts, axis=0)
voxel_down_sampled_gt_scanned_pts = PtsUtil.voxel_downsample_point_cloud(gt_scanned_pts, self.voxel_size)
result["gt_final_coverage_rate_cad"] = ReconstructionUtil.compute_coverage_rate(voxel_downsampled_cad_model_real_world_pts, voxel_down_sampled_gt_scanned_pts, self.voxel_size)
step = 1
result["real_coverage_rate_seq"] = []
for cam_to_world in cam_to_world_seq:
ControlUtil.move_to(cam_to_world)
''' get world pts '''
time.sleep(1)
view_data = CommunicateUtil.get_view_data()
if view_data is None:
Log.error("Failed to get view data")
continue
cam_pts = ViewUtil.get_pts(view_data)
shot_pts_list.append(cam_pts)
try:
ControlUtil.move_to(cam_to_world)
''' get world pts '''
time.sleep(1)
view_data = CommunicateUtil.get_view_data()
if view_data is None:
Log.error("Failed to get view data")
continue
cam_pts = ViewUtil.get_pts(view_data)
shot_pts_list.append(cam_pts)
scanned_pts = np.concatenate(shot_pts_list, axis=0)
voxel_down_sampled_scanned_pts = PtsUtil.voxel_downsample_point_cloud(scanned_pts, self.voxel_size)
voxel_down_sampled_scanned_pts_world = PtsUtil.transform_point_cloud(voxel_down_sampled_scanned_pts, first_cam_to_real_world)
curr_CR = ReconstructionUtil.compute_coverage_rate(voxel_downsampled_cad_model_real_world_pts, voxel_down_sampled_scanned_pts_world, self.voxel_size)
Log.success(f"(step {step}/{len(cam_to_world_seq)}) current coverage: {curr_CR} | gt coverage: {result['gt_final_coverage_rate_cad']}")
result["real_final_coverage_rate"] = curr_CR
result["real_coverage_rate_seq"].append(curr_CR)
step += 1
except Exception as e:
Log.error(f"Failed to move to {cam_to_world}")
Log.error(e)
#import ipdb;ipdb.set_trace()
print(idx_seq)
for idx in range(len(shot_pts_list)):
if not os.path.exists(os.path.join(temp_dir, temp_name, "shot_pts")):
os.makedirs(os.path.join(temp_dir, temp_name, "shot_pts"))
if not os.path.exists(os.path.join(temp_dir, temp_name, "render_pts")):
os.makedirs(os.path.join(temp_dir, temp_name, "render_pts"))
shot_pts = PtsUtil.transform_point_cloud(shot_pts_list[idx], first_cam_to_world)
shot_pts = PtsUtil.transform_point_cloud(shot_pts_list[idx], first_cam_to_real_world)
np.savetxt(os.path.join(temp_dir, temp_name, "shot_pts", f"{idx}.txt"), shot_pts)
np.savetxt(os.path.join(temp_dir, temp_name, "render_pts", f"{idx}.txt"), render_pts[idx])
Log.success("[Part 5/5] finish running robot")
Log.debug(result)
def run(self):
total = len(os.listdir(self.model_dir))

38
utils/control_util.py
View File

@ -19,7 +19,7 @@ class ControlUtil:
CAMERA_TO_GRIPPER:np.ndarray = np.asarray([
[0, -1, 0, 0.01],
[1, 0, 0, -0.05],
[1, 0, 0, 0],
[0, 0, 1, 0.075],
[0, 0, 0, 1]
])
@ -33,8 +33,10 @@ class ControlUtil:
@staticmethod
def connect_robot():
ControlUtil.__ser = serial.Serial(port="/dev/ttyUSB0", baudrate=115200)
ControlUtil.__fa = FrankaArm(robot_num=2)
if ControlUtil.__fa is None:
ControlUtil.__fa = FrankaArm(robot_num=2)
if ControlUtil.__ser is None:
ControlUtil.__ser = serial.Serial(port="/dev/ttyUSB0", baudrate=115200)
@staticmethod
def franka_reset() -> None:
@ -63,8 +65,7 @@ class ControlUtil:
"right": 1
}
ControlUtil.cnt_rotation += degree
print("total rot:", ControlUtil.cnt_rotation)
print(f"Table rotated {ControlUtil.cnt_rotation} degree")
if degree >= 0:
turn_angle = degree
turn_direction = turn_directions["right"]
@ -146,7 +147,6 @@ class ControlUtil:
@staticmethod
def move_to(pose: np.ndarray):
rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(pose)
print("table rot degree:", rot_degree)
exec_time = rot_degree/9
start_time = time.time()
ControlUtil.rotate_display_table(rot_degree)
@ -154,25 +154,17 @@ class ControlUtil:
end_time = time.time()
if end_time - start_time < exec_time:
time.sleep(exec_time - (end_time - start_time))
# ----------- Debug Test -------------
if __name__ == "__main__":
ControlUtil.connect_robot()
print(ControlUtil.get_curr_gripper_to_base_pose())
ControlUtil.init()
# import time
# start = time.time()
# test_pose = np.asarray([[ 0.06023737 ,-0.81328565, 0.57872715, -0.23602393],
# [-0.99107872 ,-0.11773834, -0.06230158, 0.24174289],
# [ 0.11880735 ,-0.56981127 ,-0.813138 , 0.15199069],
# [ 0. , 0. , 0. , 1. , ]])
# print(test_pose)
# rot_degree, cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(test_pose)
# print(rot_degree, cam_to_world)
# end = time.time()
# print(f"Time: {end-start}")
# ControlUtil.set_pose(test_pose)
# import ipdb; ipdb.set_trace()
# ControlUtil.move_to(test_pose)
# print("End!")
def main_test():
print(ControlUtil.get_curr_gripper_to_base_pose())
ControlUtil.init()
def rotate_back(rotation):
ControlUtil.rotate_display_table(-rotation)
rotate_back(122.0661478599)

5
utils/preprocess_util.py
View File

@ -23,6 +23,7 @@ def save_target_points(root, scene, frame_idx, target_points: np.ndarray, file_t
save_np_pts(pts_path, target_points, file_type)
def save_scan_points_indices(root, scene, frame_idx, scan_points_indices: np.ndarray, file_type="txt"):
file_type="npy"
indices_path = os.path.join(root,scene, "scan_points_indices", f"{frame_idx}.{file_type}")
if not os.path.exists(os.path.join(root,scene, "scan_points_indices")):
os.makedirs(os.path.join(root,scene, "scan_points_indices"))
@ -69,7 +70,7 @@ def save_scene_data(root, scene, file_type="txt"):
random_downsample_N = 32768
voxel_size=0.002
filter_degree = 75
min_z = 0.2
min_z = 0.25
max_z = 0.5
''' scan points '''
@ -81,7 +82,7 @@ def save_scene_data(root, scene, file_type="txt"):
''' read frame data(depth|mask|normal) '''
frame_num = DataLoadUtil.get_scene_seq_length(root, scene)
for frame_id in range(frame_num):
#print(f"[scene({scene_idx}/{scene_total})|frame({frame_id}/{frame_num})]Processing {scene} frame {frame_id}")
Log.info(f"frame({frame_id}/{frame_num})]Processing {scene} frame {frame_id}")
path = DataLoadUtil.get_path(root, scene, frame_id)
cam_info = DataLoadUtil.load_cam_info(path, binocular=True)
depth_L, depth_R = DataLoadUtil.load_depth(

2
utils/pts_util.py
View File

@ -92,7 +92,7 @@ class PtsUtil:
cam_pose,
voxel_size=0.002,
theta=45,
z_range=(0.2, 0.45),
z_range=(0.25, 0.5),
):
"""filter with z range"""
points_cam = PtsUtil.transform_point_cloud(points, np.linalg.inv(cam_pose))

4
utils/reconstruction_util.py
View File

@ -8,7 +8,7 @@ class ReconstructionUtil:
def compute_coverage_rate(target_point_cloud, combined_point_cloud, threshold=0.01):
kdtree = cKDTree(combined_point_cloud)
distances, _ = kdtree.query(target_point_cloud)
covered_points_num = np.sum(distances < threshold)
covered_points_num = np.sum(distances < threshold*2)
coverage_rate = covered_points_num / target_point_cloud.shape[0]
return coverage_rate, covered_points_num
@ -16,7 +16,7 @@ class ReconstructionUtil:
def compute_overlap_rate(new_point_cloud, combined_point_cloud, threshold=0.01):
kdtree = cKDTree(combined_point_cloud)
distances, _ = kdtree.query(new_point_cloud)
overlapping_points = np.sum(distances < threshold)
overlapping_points = np.sum(distances < threshold*2)
if new_point_cloud.shape[0] == 0:
overlap_rate = 0
else:

1
utils/view_util.py
View File

@ -108,7 +108,6 @@ class ViewUtil:
if image_id is None:
return None
else:
print(f"Image ID: {image_id}")
camera_intrinsics = CameraIntrinsics(
depth_intrinsics['width'],
depth_intrinsics['height'],