diff --git a/README.md b/README.md index a1b8c45..0765aac 100644 --- a/README.md +++ b/README.md @@ -2,13 +2,55 @@ +## A Quick-Start: Matlab Demo +Estimates 6D object poses on the sample scene data (in `data/sample`) with pre-computed object segmentation results from [Deep Learning FCN ROS Package](#deep-learning-fcn-ros-package). +1. + + ## Documentation +* [6D Pose Estimation ROS Package](#6d-pose-estimation-ros-package) * [Realsense Standalone](#realsense-standalone) * [Realsense ROS Package](#realsense-ros-package) * [Deep Learning FCN ROS Package](#deep-learning-fcn-ros-package) * [FCN Training with Marvin](#fcn-training-with-marvin) * [Evaluation Code](#evaluation-code) +## 6D Pose Estimation ROS Package +A Matlab ROS Package for estimating 6D object poses by model-fitting with ICP on RGB-D object segmentation results. + +### Dependencies +1. [Deep Learning FCN ROS Package](#deep-learning-fcn-ros-package) and all of its respective dependencies. +2. Recommended: Matlab 2015b or later + +### Compilation +1. Copy the ROS package `ros_packages/.../pose_estimation` into your catkin workspace source directory (e.g. `catkin_ws/src`) +2. Follow the instructions on the top of `pose_estimation/src/make.m` to compile ROS custom messages for Matlab +3. Compile a GPU CUDA kernel function in `pose_estimation/src`: +```shell +nvcc -ptx KNNSearch.cu +``` + +### Usage +* Start `roscore` +* To start the pose estimation service, run `pose_estimation/src/startService.m`. At each call (see service request format described in `pose_estimation/srv/EstimateObjectPose.srv`), the service: + * Calibrates the camera poses of the scene using calibration data + * Perform 3D background subtraction + * For each object in the scene, use model-fitting to estimate its 6D pose + +### Demo +1. Install all dependencies and compile this package +2. Start `roscore` in terminal +3. Create a temporary directory to be used by marvin_convnet for reading RGB-D data and saving segmentation masks + * `mkdir /path/to/your/data/tmp` +4. `rosrun marvin_convnet detect _read_directory:="/path/to/your/data/tmp"` +5. Navigate to `pose_estimation/src` +6. Edit file paths and options on the top of `demo.m` +6. Open Matlab and run: +```shell +startService.m +demo.m +``` + ## Realsense Standalone A standalone C++ executable for streaming and capturing data (RGB-D frames and 3D point clouds) in real-time using [librealsense](https://github.com/IntelRealSense/librealsense). Tested on Ubuntu 14.04 and 16.04 with an Intel® RealSense™ F200 Camera. @@ -63,7 +105,7 @@ See `ros-packages/realsense_camera` * Used for saving point clouds ### Compilation -1. Copy the ROS package `ros_packages/realsense_camera` into your catkin workspace source directory (e.g. `catkin_ws/src`) +1. Copy the ROS package `ros_packages/.../realsense_camera` into your catkin workspace source directory (e.g. `catkin_ws/src`) 2. If necessary, configure `realsense_camera/CMakeLists.txt` according to your respective dependencies 3. In your catkin workspace, compile the package with `catkin_make` 4. Source `devel/setup.sh` @@ -105,26 +147,37 @@ sudo cp cuda/include/* /usr/local/cudnn/v5/include/ * Used for saving images ### Compilation -1. Copy the ROS package `ros_packages/marvin_fcn` into your catkin workspace source directory (e.g. `catkin_ws/src`) +1. Copy the ROS package `ros_packages/.../marvin_convnet` into your catkin workspace source directory (e.g. `catkin_ws/src`) 2. If necessary, configure `realsense_camera/CMakeLists.txt` according to your respective dependencies 3. In your catkin workspace, compile the package with `catkin_make` 4. Source `devel/setup.sh` -change location of where net is loaded +### Usage +* Navigate to `models/competition/` and run bash script `./download_weights.sh` to download our trained weights for object segmentation (trained on our [training dataset](http://www.cs.princeton.edu/~andyz/apc2016)) +* Edit `marvin_convnet/src/detect.cu`: Towards the top of the file, specify the filepath to the network architecture .json file and .marvin weights. +* Create a folder called `tmp` in `apc-vision-toolbox/data` (e.g. `apc-vision-toolbox/data/tmp`). This where marvin_convnet will read/write RGB-D data. The format of the data in `tmp` follows the format of the scenes in our [datasets](http://www.cs.princeton.edu/~andyz/apc2016) and the format of the data saved by [Realsense Standalone](#realsense-standalone). +* marvin_convnet offers two services: `save_images` and `detect`. The former retrieves RGB-D data from the [Realsense ROS Package](#realsense-ros-package) and writes to disk in the `tmp` folder, while the latter reads from disk in the `tmp` folder and feeds the RGB-D data forward through the FCN and saves the response images to disk +* To start the RGB-D data saving service, run: -make sure where data is going to be read and written +```shell +rosrun marvin_convnet save_images _write_directory:="/path/to/your/data/tmp" _camera_service_name:="/realsense_camera" +``` -ros package to compute hha +* To start the FCN service, run: -`rosrun marvin_convnet detect _service_mode:=1 _write_directory:="/home/andyz/apc/toolbox/data/tmp"` +```shell +rosrun marvin_convnet detect _read_directory:="/path/to/your/data/tmp" _service_name:="/marvin_convnet" +``` -`rosrun marvin_convnet detect _service_mode:=2 _read_directory:="/home/andyz/apc/toolbox/data/tmp" _write_directory:="/home/andyz/apc/toolbox/data/tmp"` +* Example ROS service call to do object segmentation for glue bottle and expo marker box (assuming the scene's RGB-D data is in the `tmp` folder): -`rosservice call /marvin_convnet ["elmers_washable_no_run_school_glue","expo_dry_erase_board_eraser"] 0 0` +```shell +rosservice call /marvin_convnet ["elmers_washable_no_run_school_glue","expo_dry_erase_board_eraser"] 0 0 +``` ## FCN Training with Marvin -Code and models for training object segmentation using [FCNs (Fully Convolutional Networks)](https://arxiv.org/abs/1411.4038) with [Marvin](http://marvin.is/), a lightweight GPU-only neural network framework. Includes network architecture .json files in `convnet-training/models` and a Marvin data layer in `convnet-training/apc.hpp` that randomly samples images (RGB and HHA) from the segmentation training dataset [here](http://www.cs.princeton.edu/~andyz/apc2016). +Code and models for training object segmentation using [FCNs (Fully Convolutional Networks)](https://arxiv.org/abs/1411.4038) with [Marvin](http://marvin.is/), a lightweight GPU-only neural network framework. Includes network architecture .json files in `convnet-training/models` and a Marvin data layer in `convnet-training/apc.hpp` that randomly samples RGB-D images (RGB and HHA) from our [segmentation training dataset](http://www.cs.princeton.edu/~andyz/apc2016). See `convnet-training` @@ -146,19 +199,20 @@ sudo cp cuda/include/* /usr/local/cudnn/v5/include/ * Used for reading images ### Setup Instructions -1. Download segmentation training dataset from [here](http://www.cs.princeton.edu/~andyz/apc2016) -2. Specify training dataset filepath in APCData layer of network architecture in `convnet-training/models/train_shelf_color.json` -3. Navigate to `convnet-training/models/weights/` and run bash script `./download_weights.sh` to download VGG pre-trained weights on ImageNet (see [Marvin](http://marvin.is/) for more pre-trained weights) +1. Download our [segmentation training dataset](http://www.cs.princeton.edu/~andyz/apc2016) +2. Navigate to directory `convnet-training/` +2. Specify training dataset filepath in APCData layer of network architecture in `models/train_shelf_color.json` +3. Navigate to `models/weights/` and run bash script `./download_weights.sh` to download VGG pre-trained weights on ImageNet (see [Marvin](http://marvin.is/) for more pre-trained weights) 4. Navigate to `convnet-training/` and run in terminal `./compile.sh` to compile Marvin. 5. Run in terminal `./marvin train models/rgb-fcn/train_shelf_color.json models/weights/vgg16_imagenet_half.marvin` to train a segmentation model on RGB-D data with objects in the shelf (for objects in the tote, use network architecture `models/rgb-fcn/train_shelf_color.json`). ## Evaluation Code -Code used to perform the experiments in the paper - tests the full vision system on the 'Shelf & Tote' benchmark dataset. +Code used to perform the experiments in the paper; tests the full vision system on the 'Shelf & Tote' benchmark dataset. See `evaluation` ### Setup Instructions -1. Download the full 'Shelf & Tote' benchmark dataset from [here](http://www.cs.princeton.edu/~andyz/apc2016) and extract its contents to `apc-vision-toolbox/data/benchmark` (e.g. `apc-vision-toolbox/data/benchmark/office`, `apc-vision-toolbox/data/benchmark/warehouse', etc.) +1. Download our 'Shelf & Tote' benchmark dataset from [here](http://www.cs.princeton.edu/~andyz/apc2016) and extract its contents to `apc-vision-toolbox/data/benchmark` (e.g. `apc-vision-toolbox/data/benchmark/office`, `apc-vision-toolbox/data/benchmark/warehouse', etc.) 2. In `evaluation/getError.m`, change the variable `benchmarkPath` to point to the filepath of your benchmark dataset directory 3. We have provided our vision system's predictions in a saved Matlab .mat file `evaluation/predictions.mat`. To compute the accuracy of these predictions against the ground truth labels of the 'Shelf & Tote' benchmark dataset, run `evaluation/getError.m` diff --git a/data/sample/calibration/shelf/cam.poses.A.txt b/data/sample/calibration/shelf/cam.poses.A.txt new file mode 100644 index 0000000..3a4f2ca --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.A.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 9.06193058e-01 3.42857864e-02 -4.21471976e-01 1.54523532e-01 +-9.89394920e-02 9.86232759e-01 -1.32498762e-01 4.56576146e-02 + 4.11126646e-01 1.61769681e-01 8.97109498e-01 4.00954915e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.74033990e-01 1.72623355e-02 -2.25742770e-01 8.42798845e-02 +-5.44936063e-02 9.85651131e-01 -1.59756985e-01 5.61639449e-02 + 2.19745838e-01 1.67910271e-01 9.60998391e-01 1.65755026e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99941171e-01 9.65149581e-03 4.95001832e-03 -6.76798065e-04 +-8.57582876e-03 9.82882741e-01 -1.84032532e-01 6.82766391e-02 +-6.64147679e-03 1.83979255e-01 9.82907689e-01 8.37727195e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.67112011e-01 6.05468410e-03 2.54278781e-01 -1.01311126e-01 + 3.73703670e-02 9.85484988e-01 -1.65598289e-01 6.27228817e-02 +-2.51590567e-01 1.69654586e-01 9.52848103e-01 2.41726814e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.85187346e-01 9.49438568e-03 4.65137849e-01 -1.78356751e-01 + 6.75713285e-02 9.86566465e-01 -1.48730377e-01 5.92754124e-02 +-4.60301507e-01 1.63084230e-01 8.72654603e-01 5.42887834e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.08533859e-01 -9.44479893e-02 4.06996074e-01 -1.42541407e-01 + 9.68736841e-02 9.95188207e-01 1.46942527e-02 5.83083812e-03 +-4.06425536e-01 2.60769830e-02 9.13311707e-01 3.42312243e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.75567915e-01 -4.42249756e-02 2.15200824e-01 -7.68780799e-02 + 4.49305673e-02 9.98988808e-01 1.61447263e-03 7.44182428e-03 +-2.15054615e-01 8.09406742e-03 9.76568481e-01 1.14108454e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.77786599e-01 5.05112051e-02 -2.03425627e-01 7.39230849e-02 +-5.01212684e-02 9.98718104e-01 7.07162812e-03 -5.35480146e-03 + 2.03522053e-01 3.28140726e-03 9.79064863e-01 6.79063607e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.06949455e-01 9.70696317e-02 -4.09902638e-01 1.50787138e-01 +-9.18811102e-02 9.95243091e-01 3.23890512e-02 -1.23167106e-02 + 4.11096762e-01 8.28707713e-03 9.11554045e-01 3.04464373e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.04354087e-01 1.57968695e-01 -3.96471408e-01 1.47415385e-01 +-8.15376054e-02 9.75816796e-01 2.02813212e-01 -6.94670376e-02 + 4.18921598e-01 -1.51087628e-01 8.95364297e-01 3.55340376e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.76539943e-01 7.69014782e-02 -2.01136525e-01 7.61511874e-02 +-4.03811507e-02 9.82885646e-01 1.79736388e-01 -6.45838775e-02 + 2.11516197e-01 -1.67397638e-01 9.62932463e-01 1.10402609e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99998641e-01 -1.56633748e-03 -5.15200283e-04 -6.83942273e-05 + 1.63268064e-03 9.84298747e-01 1.76503003e-01 -5.78220348e-02 + 2.30647723e-04 -1.76503604e-01 9.84299967e-01 5.96666405e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.75575029e-01 -8.57247925e-02 2.02248911e-01 -7.19359239e-02 + 5.06092233e-02 9.83654260e-01 1.72809152e-01 -5.54876912e-02 +-2.13757031e-01 -1.58352634e-01 9.63966999e-01 1.39479082e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 8.99900305e-01 -1.58189084e-01 4.06393473e-01 -1.49015967e-01 + 8.56453963e-02 9.77850486e-01 1.90979825e-01 -5.60021794e-02 +-4.27602978e-01 -1.37057073e-01 8.93516117e-01 4.19620656e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.B.txt b/data/sample/calibration/shelf/cam.poses.B.txt new file mode 100644 index 0000000..a808e71 --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.B.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 9.05247257e-01 2.89516540e-02 -4.23897635e-01 1.49310311e-01 +-9.97035745e-02 9.84292822e-01 -1.45694330e-01 5.84530536e-02 + 4.13021308e-01 1.74153502e-01 8.93914961e-01 4.00667733e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.76027409e-01 1.84703952e-02 -2.16862495e-01 7.64990842e-02 +-5.62802670e-02 9.83922320e-01 -1.69497492e-01 6.38484133e-02 + 2.10245164e-01 1.77639277e-01 9.61374671e-01 1.52328476e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99939471e-01 1.01612775e-02 4.21927314e-03 -3.39140620e-03 +-9.23715004e-03 9.83659092e-01 -1.79803963e-01 6.77236939e-02 +-5.97736435e-03 1.79754106e-01 9.83693414e-01 7.54003432e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.73498541e-01 -4.48275288e-03 2.28649285e-01 -8.80713433e-02 + 4.76295117e-02 9.81858050e-01 -1.83538003e-01 7.15631809e-02 +-2.23678386e-01 1.89564432e-01 9.56050891e-01 2.05026384e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.96544327e-01 -1.46288801e-02 4.42712397e-01 -1.66263642e-01 + 9.87479638e-02 9.80903326e-01 -1.67563434e-01 7.03188472e-02 +-4.31806797e-01 1.93944994e-01 8.80867884e-01 4.99335242e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.05536125e-01 -8.34244741e-02 4.15986397e-01 -1.53295248e-01 + 8.94276848e-02 9.95980412e-01 5.07022091e-03 4.25375109e-03 +-4.14737284e-01 3.26094322e-02 9.09356701e-01 3.79101188e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.76479572e-01 -4.14692150e-02 2.11584378e-01 -7.83460624e-02 + 4.43033703e-02 9.98980503e-01 -8.66982143e-03 5.93579989e-03 +-2.11009138e-01 1.78398046e-02 9.77321280e-01 1.13443493e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.78630296e-01 5.52592281e-02 -1.98063527e-01 6.69496630e-02 +-5.35004321e-02 9.98466509e-01 1.42244501e-02 -4.07535827e-03 + 1.98545830e-01 -3.32399352e-03 9.80085968e-01 6.43911432e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.14591958e-01 1.00486098e-01 -3.91693878e-01 1.33637585e-01 +-9.54076667e-02 9.94908817e-01 3.24626480e-02 -3.41561345e-03 + 3.92961738e-01 7.68052220e-03 9.19522747e-01 2.85597281e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.05477435e-01 1.57156091e-01 -3.94224020e-01 1.41790888e-01 +-7.92369450e-02 9.75178771e-01 2.06755584e-01 -6.22180308e-02 + 4.16931794e-01 -1.55975409e-01 8.95454941e-01 3.54998236e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.77589946e-01 8.00082873e-02 -1.94721781e-01 6.67172086e-02 +-4.35950010e-02 9.81853397e-01 1.84562680e-01 -5.99440969e-02 + 2.05954787e-01 -1.71937724e-01 9.63337970e-01 1.09906624e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99960437e-01 -2.84680346e-03 8.42732460e-03 -5.87763588e-03 + 1.34444403e-03 9.84890233e-01 1.73174543e-01 -5.68454979e-02 +-8.79298358e-03 -1.73156362e-01 9.84855095e-01 4.41092430e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.73023063e-01 -8.22832744e-02 2.15535571e-01 -8.16399246e-02 + 4.55981708e-02 9.84396366e-01 1.69954706e-01 -5.81909463e-02 +-2.26156862e-01 -1.55541820e-01 9.61592333e-01 1.35834602e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.01818422e-01 -1.67823869e-01 3.98194279e-01 -1.44817903e-01 + 9.78829696e-02 9.76885125e-01 1.90037830e-01 -6.06841190e-02 +-4.20882951e-01 -1.32403177e-01 8.97400100e-01 4.05301648e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.C.txt b/data/sample/calibration/shelf/cam.poses.C.txt new file mode 100644 index 0000000..9dab33f --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.C.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 9.05908816e-01 3.14809071e-02 -4.22301041e-01 1.51247836e-01 +-9.82954066e-02 9.85628293e-01 -1.37385886e-01 5.58329992e-02 + 4.11906822e-01 1.65969338e-01 8.95983788e-01 3.82842343e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.74931187e-01 1.82636626e-02 -2.21755766e-01 7.92246607e-02 +-5.61808402e-02 9.84539537e-01 -1.65908449e-01 6.16499334e-02 + 2.15297224e-01 1.74207746e-01 9.60884367e-01 1.51701999e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99928356e-01 1.14683213e-02 3.42935498e-03 -1.72475132e-03 +-1.06847499e-02 9.84296704e-01 -1.76198280e-01 6.10500909e-02 +-5.39620129e-03 1.76149015e-01 9.84348722e-01 7.04444281e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.73296241e-01 1.25516066e-03 2.29549237e-01 -8.38346593e-02 + 4.13878892e-02 9.82637810e-01 -1.80858999e-01 6.55817534e-02 +-2.25790767e-01 1.85529942e-01 9.56345738e-01 1.84976195e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.94317694e-01 -8.79189098e-03 4.47346135e-01 -1.67230523e-01 + 8.75448802e-02 9.83920695e-01 -1.55679029e-01 5.25666021e-02 +-4.38784407e-01 1.78389374e-01 8.80707372e-01 4.77181808e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.05211214e-01 -9.01444556e-02 4.15291025e-01 -1.47940969e-01 + 9.77347137e-02 9.95208010e-01 2.99049425e-03 -8.44901630e-05 +-4.13570531e-01 3.78813205e-02 9.09683693e-01 3.35332369e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.75568069e-01 -5.06066846e-02 2.13789397e-01 -7.51388838e-02 + 5.19660645e-02 9.98648577e-01 -7.39698087e-04 -1.37658672e-03 +-2.13463044e-01 1.18314194e-02 9.76879494e-01 7.79036996e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.75274614e-01 4.83910309e-02 -2.15633334e-01 7.57471475e-02 +-4.73042630e-02 9.98828436e-01 1.02010704e-02 -7.03489397e-04 + 2.15874346e-01 2.51530947e-04 9.76421120e-01 8.38561390e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.05161799e-01 9.36912616e-02 -4.14613152e-01 1.49405445e-01 +-8.88794740e-02 9.95561952e-01 3.09328122e-02 -3.93826595e-03 + 4.15671213e-01 8.85139890e-03 9.09471877e-01 3.05057361e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.04874018e-01 1.56592989e-01 -3.95830326e-01 1.40858566e-01 +-7.65804279e-02 9.74591765e-01 2.10490688e-01 -6.51845386e-02 + 4.18734342e-01 -1.60154699e-01 8.93874725e-01 3.34238915e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.76782743e-01 7.90275588e-02 -1.99123372e-01 7.09116650e-02 +-4.05697107e-02 9.80890829e-01 1.90282105e-01 -6.49971726e-02 + 2.10355820e-01 -1.77785899e-01 9.61323360e-01 1.10774585e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99981604e-01 -5.20959601e-03 -3.10678213e-03 1.31531196e-03 + 5.67130135e-03 9.84682798e-01 1.74263085e-01 -6.04827883e-02 + 2.15135465e-03 -1.74277499e-01 9.84694229e-01 3.15738841e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.76429800e-01 -9.44103825e-02 1.94091537e-01 -6.82034137e-02 + 6.01526425e-02 9.82662429e-01 1.75374484e-01 -6.27464858e-02 +-2.07283633e-01 -1.59565754e-01 9.65179914e-01 1.06606852e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.06503003e-01 -1.68897378e-01 3.86944417e-01 -1.39766585e-01 + 1.01521595e-01 9.76806059e-01 1.88529276e-01 -6.64892773e-02 +-4.09811751e-01 -1.31619141e-01 9.02624357e-01 3.59190944e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.D.txt b/data/sample/calibration/shelf/cam.poses.D.txt new file mode 100644 index 0000000..a3c8d8a --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.D.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 8.76500255e-01 3.21493260e-02 -4.80326685e-01 1.93286223e-01 +-6.42866406e-02 9.96647691e-01 -5.06024463e-02 -5.69207262e-03 + 4.77089647e-01 7.52316460e-02 8.75628727e-01 4.88054995e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.68303233e-01 9.26866279e-03 -2.49605571e-01 1.00577139e-01 +-3.77479648e-02 9.93263836e-01 -1.09553840e-01 2.30592294e-02 + 2.46908769e-01 1.15503440e-01 9.62130456e-01 1.41985487e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99832839e-01 4.09820820e-03 -1.78184947e-02 1.27556946e-02 +-6.25371790e-03 9.92429940e-01 -1.22652785e-01 3.30183370e-02 + 1.71809510e-02 1.22743714e-01 9.92289673e-01 2.46179578e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.67357049e-01 -3.66197682e-04 2.53417058e-01 -9.97796817e-02 + 3.20550012e-02 9.92143542e-01 -1.20928366e-01 3.86161659e-02 +-2.51381814e-01 1.25104191e-01 9.59768787e-01 1.64364308e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 9.04489314e-01 -4.81691393e-03 4.26469083e-01 -1.52146803e-01 + 6.36389044e-02 9.90266249e-01 -1.23785484e-01 4.65178431e-02 +-4.21721675e-01 1.39102672e-01 8.95991783e-01 3.99107456e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 8.98225129e-01 -8.71090918e-02 4.30817390e-01 -1.57886653e-01 + 8.73981477e-02 9.95989095e-01 1.91647161e-02 2.65013944e-03 +-4.30758843e-01 2.04384122e-02 9.02235607e-01 3.82674616e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.73716730e-01 -4.52072741e-02 2.23230894e-01 -8.22811364e-02 + 4.56718788e-02 9.98951735e-01 3.08385812e-03 2.97945118e-03 +-2.23136302e-01 7.19257010e-03 9.74760718e-01 1.02842549e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.75528473e-01 5.12795129e-02 -2.13809751e-01 8.05661068e-02 +-4.99318349e-02 9.98684066e-01 1.17024867e-02 -5.60323162e-03 + 2.14128489e-01 -7.40195788e-04 9.76805222e-01 9.69837316e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.05923714e-01 1.03140000e-01 -4.10687674e-01 1.53210402e-01 +-9.49266998e-02 9.94663995e-01 4.04036861e-02 -1.30103391e-02 + 4.12663479e-01 2.38256817e-03 9.10880440e-01 3.23196395e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.05369660e-01 1.55107052e-01 -3.95281647e-01 1.49172714e-01 +-7.69210379e-02 9.75405154e-01 2.06562194e-01 -7.24804730e-02 + 4.17599009e-01 -1.56609669e-01 8.95033228e-01 3.48634671e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.76464425e-01 7.37007834e-02 -2.02695389e-01 7.76871718e-02 +-3.47471323e-02 9.81283564e-01 1.89407509e-01 -6.74554910e-02 + 2.12861136e-01 -1.77906611e-01 9.60749382e-01 1.26689884e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99958255e-01 -7.38935298e-03 -5.37458339e-03 3.21979995e-03 + 8.24730428e-03 9.83120246e-01 1.82774628e-01 -6.24186066e-02 + 3.93327550e-03 -1.82811324e-01 9.83140147e-01 5.45923218e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.76060460e-01 -9.12928351e-02 1.97412250e-01 -6.97548657e-02 + 5.38336119e-02 9.80806725e-01 1.87403601e-01 -6.07976967e-02 +-2.10731869e-01 -1.72289831e-01 9.62241287e-01 1.56846444e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.05543497e-01 -1.62480932e-01 3.91906778e-01 -1.42094428e-01 + 9.17694230e-02 9.76903396e-01 1.92971834e-01 -6.11302858e-02 +-4.14209306e-01 -1.38779330e-01 8.99539298e-01 3.93474008e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.E.txt b/data/sample/calibration/shelf/cam.poses.E.txt new file mode 100644 index 0000000..d0e5869 --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.E.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 8.79715854e-01 2.68502104e-02 -4.74741069e-01 1.84910856e-01 +-8.45544576e-02 9.91325853e-01 -1.00616088e-01 3.24667139e-02 + 4.67921532e-01 1.28655041e-01 8.74355374e-01 4.95379226e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.71053667e-01 1.91239801e-02 -2.38094622e-01 9.13026706e-02 +-5.00615094e-02 9.90945976e-01 -1.24578963e-01 3.51294927e-02 + 2.33556462e-01 1.32892235e-01 9.63219099e-01 1.41858355e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99882633e-01 2.28621529e-03 -1.51490294e-02 8.90195213e-03 +-4.54758270e-03 9.88527756e-01 -1.50970840e-01 4.93370484e-02 + 1.46300842e-02 1.51022012e-01 9.88422133e-01 4.81248613e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.77629007e-01 -5.36568026e-03 2.10268245e-01 -7.80252799e-02 + 3.51219882e-02 9.89803803e-01 -1.38039406e-01 4.55194285e-02 +-2.07383633e-01 1.42336366e-01 9.67849362e-01 1.46080903e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 9.07667426e-01 -1.35531893e-02 4.19471281e-01 -1.54332223e-01 + 7.19892762e-02 9.89692942e-01 -1.23795901e-01 4.41700455e-02 +-4.13469937e-01 1.42562941e-01 8.99287729e-01 4.18227947e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.03717661e-01 -9.30683197e-02 4.17890748e-01 -1.54479441e-01 + 9.31396347e-02 9.95446606e-01 2.02747015e-02 -2.39934484e-03 +-4.17874859e-01 2.05995858e-02 9.08271027e-01 3.69119144e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.76972037e-01 -4.65494246e-02 2.08227735e-01 -7.65678610e-02 + 4.54598381e-02 9.98915936e-01 1.00177405e-02 -1.82121715e-03 +-2.08468323e-01 -3.21053188e-04 9.78029067e-01 9.26402182e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.74892287e-01 5.10416605e-02 -2.16748189e-01 7.89281020e-02 +-4.98874979e-02 9.98696484e-01 1.07968139e-02 -2.53165009e-03 + 2.17016741e-01 2.87294214e-04 9.76167840e-01 8.48991539e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 8.99740903e-01 9.42209605e-02 -4.26132278e-01 1.55159370e-01 +-9.18748638e-02 9.95428169e-01 2.61107310e-02 -3.88333867e-03 + 4.26644251e-01 1.56579523e-02 9.04283977e-01 3.39152043e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.03831845e-01 1.61096618e-01 -3.96403677e-01 1.46434372e-01 +-7.97650123e-02 9.73615024e-01 2.13802076e-01 -7.04878171e-02 + 4.20387367e-01 -1.61621981e-01 8.92834137e-01 3.61746137e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.75425947e-01 8.27386531e-02 -2.04202195e-01 7.43533900e-02 +-4.25392372e-02 9.80095560e-01 1.93915206e-01 -6.80059983e-02 + 2.16181947e-01 -1.80463318e-01 9.59530279e-01 1.31354177e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99972068e-01 -4.75468287e-03 -5.76677337e-03 1.83953957e-03 + 5.72118179e-03 9.83422592e-01 1.81238166e-01 -6.54015058e-02 + 4.80944522e-03 -1.81266097e-01 9.83422326e-01 5.37979695e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.77466333e-01 -8.89311807e-02 1.91444023e-01 -6.99733147e-02 + 5.46878074e-02 9.82646239e-01 1.77244498e-01 -6.40954376e-02 +-2.03884312e-01 -1.62780876e-01 9.65367067e-01 1.27031130e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.07710997e-01 -1.66556251e-01 3.85123046e-01 -1.41064533e-01 + 9.82431631e-02 9.76692115e-01 1.90842325e-01 -6.29435929e-02 +-4.07932624e-01 -1.35393971e-01 9.02917187e-01 3.71356949e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.F.txt b/data/sample/calibration/shelf/cam.poses.F.txt new file mode 100644 index 0000000..3bca840 --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.F.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 8.99520694e-01 1.52862351e-02 -4.36610641e-01 1.60288002e-01 +-8.89651187e-02 9.84856000e-01 -1.48808157e-01 6.32101398e-02 + 4.27723893e-01 1.72699135e-01 8.87258294e-01 4.27666966e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.74596166e-01 1.49661024e-02 -2.23468854e-01 8.13718240e-02 +-5.66395711e-02 9.81801995e-01 -1.81264453e-01 7.21553441e-02 + 2.16689344e-01 1.89316821e-01 9.57708134e-01 1.56151146e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99993410e-01 3.39670551e-03 1.28168013e-03 -1.97253781e-03 +-3.09915433e-03 9.82555761e-01 -1.85942387e-01 6.94215231e-02 +-1.89091373e-03 1.85937189e-01 9.82559813e-01 7.15595432e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.77050513e-01 -1.17938286e-02 2.12680984e-01 -7.76397976e-02 + 5.02612001e-02 9.83037013e-01 -1.76386062e-01 6.41584005e-02 +-2.06993013e-01 1.83027694e-01 9.61069590e-01 1.40564800e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.97920999e-01 -1.25176146e-02 4.39978624e-01 -1.64848336e-01 + 9.50693763e-02 9.81516194e-01 -1.66095678e-01 6.21077015e-02 +-4.29767023e-01 1.90969290e-01 8.82514043e-01 4.40903289e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.04938237e-01 -9.06201487e-02 4.15782125e-01 -1.53878824e-01 + 9.74667208e-02 9.95227319e-01 4.77722583e-03 9.56782533e-04 +-4.14230643e-01 3.62018260e-02 9.09451704e-01 3.67931647e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.76525675e-01 -5.05627349e-02 2.09382463e-01 -7.50613935e-02 + 5.35057356e-02 9.98532115e-01 -8.41144926e-03 1.35286186e-03 +-2.08649808e-01 1.94171589e-02 9.77797644e-01 7.50166200e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.73754304e-01 4.37916696e-02 -2.23349155e-01 8.07298896e-02 +-4.20933372e-02 9.99037208e-01 1.23615379e-02 -2.50231345e-03 + 2.23675449e-01 -2.63558939e-03 9.74660119e-01 7.25422758e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.04231193e-01 8.84856627e-02 -4.17775342e-01 1.51333946e-01 +-8.21094390e-02 9.96068444e-01 3.32519874e-02 -5.29903847e-03 + 4.19075159e-01 4.23581470e-03 9.07941666e-01 2.89829387e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.07905566e-01 1.51769894e-01 -3.90734414e-01 1.39000388e-01 +-7.40106119e-02 9.75547263e-01 2.06954021e-01 -6.47541180e-02 + 4.12589278e-01 -1.58976215e-01 8.96937373e-01 3.13822671e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.77106666e-01 7.28533675e-02 -1.99887342e-01 6.80169060e-02 +-3.55252797e-02 9.82221323e-01 1.84334551e-01 -6.16635045e-02 + 2.09763002e-01 -1.73013465e-01 9.62323139e-01 9.93356626e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99964519e-01 -8.41892792e-03 2.88137632e-04 -3.17451422e-03 + 8.23803966e-03 9.84479272e-01 1.75307439e-01 -6.28718995e-02 +-1.75956622e-03 -1.75298845e-01 9.84513697e-01 3.61575457e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.74989104e-01 -9.51527732e-02 2.00853669e-01 -7.42237950e-02 + 5.80044085e-02 9.81337885e-01 1.83334241e-01 -6.53305654e-02 +-2.14550076e-01 -1.67098489e-01 9.62313026e-01 1.35896722e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.01219687e-01 -1.65275267e-01 4.00608489e-01 -1.51731154e-01 + 9.47298539e-02 9.77194515e-01 1.90045087e-01 -7.07097936e-02 +-4.22882170e-01 -1.33322790e-01 8.96323437e-01 4.17255834e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.G.txt b/data/sample/calibration/shelf/cam.poses.G.txt new file mode 100644 index 0000000..fc54e44 --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.G.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 9.18468118e-01 4.41337786e-02 -3.93024841e-01 1.40482960e-01 +-9.91431972e-02 9.87716374e-01 -1.20776615e-01 4.08580339e-02 + 3.82866743e-01 1.49895210e-01 9.11561563e-01 3.21357090e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.81425063e-01 1.88478578e-02 -1.90917796e-01 6.49441163e-02 +-4.78670957e-02 9.87745546e-01 -1.48551263e-01 5.26799999e-02 + 1.85778329e-01 1.54930613e-01 9.70300426e-01 1.14021717e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99793831e-01 9.36207744e-03 1.80179483e-02 -1.04050935e-02 +-6.34745396e-03 9.86994711e-01 -1.60627367e-01 6.05957685e-02 +-1.92874255e-02 1.60479883e-01 9.86850648e-01 5.69430016e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.71405344e-01 -1.98117488e-03 2.37418896e-01 -9.20495691e-02 + 4.46632655e-02 9.83637894e-01 -1.74532764e-01 6.93117389e-02 +-2.33188443e-01 1.80145963e-01 9.55599593e-01 1.85767564e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.76909847e-01 1.45973035e-03 4.80652670e-01 -1.87455198e-01 + 8.98322717e-02 9.81877526e-01 -1.66873265e-01 7.51158647e-02 +-4.72185644e-01 1.89510930e-01 8.60886941e-01 5.58405220e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.06848692e-01 -8.55387263e-02 4.12684597e-01 -1.49699227e-01 + 8.37801406e-02 9.96232661e-01 2.23913422e-02 1.58159224e-03 +-4.13045201e-01 1.42692142e-02 9.10598732e-01 3.47693532e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.75573828e-01 -4.88963501e-02 2.14160812e-01 -8.02250002e-02 + 4.69751133e-02 9.98797187e-01 1.40541391e-02 -1.22087937e-03 +-2.14590413e-01 -3.65062183e-03 9.76697306e-01 9.45141285e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.75678059e-01 4.91459687e-02 -2.13628178e-01 8.01607397e-02 +-4.82969032e-02 9.98790700e-01 9.19498406e-03 -2.70323844e-03 + 2.13821734e-01 1.34623525e-03 9.76871769e-01 7.54139148e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.07261421e-01 9.88211002e-02 -4.08792250e-01 1.53133307e-01 +-9.56397795e-02 9.95014383e-01 2.82738571e-02 -9.43859755e-03 + 4.09548222e-01 1.34450209e-02 9.12189391e-01 2.99322843e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.06062007e-01 1.57784368e-01 -3.92626709e-01 1.47884944e-01 +-8.10924086e-02 9.75426879e-01 2.04857085e-01 -7.42444174e-02 + 4.15301891e-01 -1.53774176e-01 8.96592350e-01 3.52966568e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.76188526e-01 7.86438948e-02 -2.02166018e-01 7.72955319e-02 +-3.96033732e-02 9.80916715e-01 1.90352230e-01 -7.10248826e-02 + 2.13278067e-01 -1.77813207e-01 9.60674206e-01 1.30391900e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99982955e-01 -4.40234919e-03 -3.83524038e-03 1.98380870e-03 + 5.03439087e-03 9.82820365e-01 1.84496031e-01 -6.59995051e-02 + 2.95713640e-03 -1.84512194e-01 9.82825776e-01 5.65754417e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.74734882e-01 -8.21741517e-02 2.07700069e-01 -7.63289686e-02 + 4.27544173e-02 9.81314630e-01 1.87599723e-01 -6.45829558e-02 +-2.19234964e-01 -1.73979898e-01 9.60034908e-01 1.65721464e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.12804771e-01 -1.71162747e-01 3.70797472e-01 -1.32891995e-01 + 9.77938698e-02 9.73129197e-01 2.08460849e-01 -7.01233054e-02 +-3.96514577e-01 -1.54022337e-01 9.05015641e-01 3.74892415e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.H.txt b/data/sample/calibration/shelf/cam.poses.H.txt new file mode 100644 index 0000000..e882286 --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.H.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 9.01974242e-01 3.57149035e-02 -4.30310251e-01 1.59646952e-01 +-9.58896850e-02 9.88256553e-01 -1.18971227e-01 4.15662380e-02 + 4.21007879e-01 1.48571297e-01 8.94806647e-01 3.83347954e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.74564803e-01 2.16655083e-02 -2.23056162e-01 8.27076016e-02 +-5.79083398e-02 9.85859046e-01 -1.57253187e-01 5.85067130e-02 + 2.16494965e-01 1.66170233e-01 9.62038141e-01 1.45570896e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99979249e-01 6.43086832e-03 -3.80972793e-04 -3.09099872e-04 +-6.40408138e-03 9.85912387e-01 -1.67139920e-01 5.98323312e-02 +-6.99249022e-04 1.67138892e-01 9.85933112e-01 5.92017965e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.75005744e-01 -4.66212259e-03 2.22130734e-01 -8.62357866e-02 + 4.36072867e-02 9.84349432e-01 -1.70747184e-01 6.56376150e-02 +-2.17858218e-01 1.76166003e-01 9.59949653e-01 1.84033502e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.94961038e-01 -1.67530927e-02 4.45829647e-01 -1.71864006e-01 + 9.70258397e-02 9.82685815e-01 -1.57843518e-01 6.49942619e-02 +-4.35466102e-01 1.84520794e-01 8.81091000e-01 4.82356568e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.01662535e-01 -9.29065965e-02 4.22342323e-01 -1.59715809e-01 + 9.61582454e-02 9.95272456e-01 1.36502758e-02 2.29389167e-04 +-4.21613882e-01 2.83037545e-02 9.06333621e-01 3.96393150e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.75474946e-01 -4.85807076e-02 2.14682428e-01 -8.12521346e-02 + 4.92301445e-02 9.98784758e-01 2.32388706e-03 -3.35340143e-04 +-2.14534433e-01 8.30195336e-03 9.76681143e-01 1.15458507e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.75869250e-01 4.98507104e-02 -2.12589070e-01 7.75434443e-02 +-4.96492349e-02 9.98746913e-01 6.28951167e-03 1.63558626e-04 + 2.12636214e-01 4.41714366e-03 9.77121451e-01 7.89906340e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.06499664e-01 9.82272042e-02 -4.10621208e-01 1.50680963e-01 +-9.61434738e-02 9.95033609e-01 2.57788508e-02 -3.38974754e-03 + 4.11114087e-01 1.61100297e-02 9.11441537e-01 3.21444418e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.02686858e-01 1.62066998e-01 -3.98610994e-01 1.47147000e-01 +-8.48009451e-02 9.75195681e-01 2.04455821e-01 -6.97032080e-02 + 4.21859261e-01 -1.50756994e-01 8.94039760e-01 3.59647757e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.75126383e-01 7.77147005e-02 -2.07578812e-01 7.55741056e-02 +-3.95917050e-02 9.82527402e-01 1.81858189e-01 -6.48367862e-02 + 2.18084926e-01 -1.69116319e-01 9.61165249e-01 1.38361500e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99976252e-01 -6.36138602e-03 -2.65111060e-03 4.99641656e-04 + 6.72498943e-03 9.84799427e-01 1.73565154e-01 -6.17797934e-02 + 1.50669725e-03 -1.73578861e-01 9.84818820e-01 5.26094426e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.75160735e-01 -8.72515181e-02 2.03589569e-01 -7.67426146e-02 + 5.09168907e-02 9.82834233e-01 1.77325520e-01 -6.39298862e-02 +-2.15566719e-01 -1.62554737e-01 9.62863930e-01 1.56269484e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.02195120e-01 -1.62081694e-01 3.99716762e-01 -1.50333838e-01 + 9.43173779e-02 9.78417444e-01 1.83857386e-01 -6.24805831e-02 +-4.20889770e-01 -1.28175000e-01 8.98010563e-01 4.23809769e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.I.txt b/data/sample/calibration/shelf/cam.poses.I.txt new file mode 100644 index 0000000..2e2e6b0 --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.I.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 9.08218753e-01 3.16634475e-02 -4.17296205e-01 1.47230220e-01 +-1.07924564e-01 9.81126679e-01 -1.60445408e-01 7.50348308e-02 + 4.04340185e-01 1.90756040e-01 8.94494912e-01 3.28997515e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.77207660e-01 1.65216481e-02 -2.11641738e-01 7.25959601e-02 +-5.74835022e-02 9.80314435e-01 -1.88889530e-01 8.15132662e-02 + 2.04354685e-01 1.96750204e-01 9.58920497e-01 1.30829146e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99889882e-01 4.73917625e-03 1.40628517e-02 -1.19117771e-02 +-1.75003550e-03 9.78679776e-01 -2.05384600e-01 8.62502940e-02 +-1.47363823e-02 2.05337373e-01 9.78580299e-01 8.67891841e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.69688603e-01 -6.74120245e-03 2.44251038e-01 -1.00664268e-01 + 5.79445041e-02 9.77449162e-01 -2.03065432e-01 8.37241363e-02 +-2.37374067e-01 2.11063240e-01 9.48211928e-01 2.30717067e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.88670094e-01 -7.46213002e-03 4.58486402e-01 -1.84035602e-01 + 1.05315915e-01 9.76460424e-01 -1.88238145e-01 7.86108008e-02 +-4.46289169e-01 2.15567525e-01 8.68537057e-01 5.28843467e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.05032085e-01 -8.98567684e-02 4.15743535e-01 -1.54982190e-01 + 9.35972094e-02 9.95544644e-01 1.14203745e-02 -3.16576958e-03 +-4.14917447e-01 2.85766293e-02 9.09410187e-01 3.55820388e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.75301509e-01 -4.83873076e-02 2.15512494e-01 -8.16079826e-02 + 4.85751857e-02 9.98809714e-01 4.42786853e-03 -2.97079696e-03 +-2.15470225e-01 6.15005254e-03 9.76491044e-01 9.74481487e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.74790308e-01 4.50988836e-02 -2.18517610e-01 7.85674956e-02 +-4.36976129e-02 9.98981530e-01 1.12436805e-02 4.89713845e-04 + 2.18802134e-01 -1.41153287e-03 9.75768227e-01 7.36866626e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.03306064e-01 9.09019394e-02 -4.19255282e-01 1.52443880e-01 +-8.53023377e-02 9.95837020e-01 3.21269595e-02 -3.52009570e-03 + 4.20430333e-01 6.74297830e-03 9.07299767e-01 3.00408346e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.03843875e-01 1.58476838e-01 -3.97430925e-01 1.44253678e-01 +-8.03528677e-02 9.75219461e-01 2.06132045e-01 -6.90962801e-02 + 4.20249528e-01 -1.54376472e-01 8.94180205e-01 3.22019987e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.75407265e-01 8.30097311e-02 -2.04181419e-01 7.11135224e-02 +-4.38963469e-02 9.80973529e-01 1.89113845e-01 -6.48113361e-02 + 2.15994856e-01 -1.75500200e-01 9.60492531e-01 1.08780040e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99987584e-01 -3.41346514e-03 -3.63055588e-03 -1.04797776e-03 + 4.01234032e-03 9.83590969e-01 1.80368251e-01 -6.65918484e-02 + 2.95530124e-03 -1.80380579e-01 9.83592453e-01 3.92056517e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.75067074e-01 -7.94837428e-02 2.07187198e-01 -8.05219905e-02 + 4.21518285e-02 9.82994260e-01 1.78733064e-01 -6.72086227e-02 +-2.17870200e-01 -1.65543407e-01 9.61835722e-01 1.46805184e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.03855030e-01 -1.58480988e-01 3.97403903e-01 -1.52260467e-01 + 8.72968012e-02 9.77636992e-01 1.91324804e-01 -6.68233048e-02 +-4.18838100e-01 -1.38237797e-01 8.97476995e-01 4.13933458e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.J.txt b/data/sample/calibration/shelf/cam.poses.J.txt new file mode 100644 index 0000000..07d8069 --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.J.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 9.08883846e-01 5.19442766e-02 -4.13801820e-01 1.53881652e-01 +-1.02156161e-01 9.89715177e-01 -1.00139835e-01 2.55472653e-02 + 4.04344251e-01 1.33287884e-01 9.04842565e-01 3.64041873e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.80720132e-01 2.78753425e-02 -1.93419201e-01 6.62728787e-02 +-6.32458679e-02 9.81779196e-01 -1.79191438e-01 7.25744373e-02 + 1.84899925e-01 1.87969616e-01 9.64613623e-01 1.16246141e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99840041e-01 1.37251309e-02 1.14679029e-02 -6.14222810e-03 +-1.08286950e-02 9.74846250e-01 -2.22615203e-01 9.89919441e-02 +-1.42348650e-02 2.22455411e-01 9.74838940e-01 7.94047715e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.67860016e-01 1.52647512e-02 2.51025849e-01 -1.03276175e-01 + 4.23987632e-02 9.73964662e-01 -2.22699758e-01 1.00335176e-01 +-2.47889762e-01 2.26185377e-01 9.42014247e-01 2.02937528e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.90567270e-01 -9.41900428e-03 4.54754021e-01 -1.72037631e-01 + 8.31665865e-02 9.86303193e-01 -1.42440622e-01 5.70245090e-02 +-4.47183694e-01 1.64673296e-01 8.79152688e-01 4.84118573e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.03202126e-01 -8.78435498e-02 4.20130255e-01 -1.54166045e-01 + 8.71225563e-02 9.95977329e-01 2.09480415e-02 -6.78983755e-04 +-4.20280360e-01 1.76825062e-02 9.07221995e-01 3.56556689e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.75505329e-01 -4.42160610e-02 2.15486179e-01 -7.96860279e-02 + 4.49925080e-02 9.98986474e-01 1.30317204e-03 4.06906851e-03 +-2.15325399e-01 8.42401235e-03 9.76506021e-01 1.00316931e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.77178720e-01 5.03044106e-02 -2.06376391e-01 7.81866422e-02 +-4.92518682e-02 9.98733919e-01 1.02378167e-02 -2.19337873e-03 + 2.06630109e-01 1.60246179e-04 9.78419119e-01 6.26952841e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.08111987e-01 9.33836927e-02 -4.08181460e-01 1.57350094e-01 +-9.35924129e-02 9.95419423e-01 1.95098263e-02 -3.33207693e-03 + 4.08133653e-01 2.04855806e-02 9.12692315e-01 2.99556807e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.01929696e-01 1.47844916e-01 -4.05788989e-01 1.60146327e-01 +-7.03593694e-02 9.77328811e-01 1.99694653e-01 -7.34819560e-02 + 4.26113109e-01 -1.51559480e-01 8.91884153e-01 3.55830954e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.74486396e-01 6.81184463e-02 -2.13860097e-01 8.57988981e-02 +-2.76630505e-02 9.82019683e-01 1.86740721e-01 -7.04682898e-02 + 2.22735312e-01 -1.76060270e-01 9.58849186e-01 1.28035913e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99958197e-01 -8.63989040e-03 -2.99266142e-03 1.47067914e-03 + 9.03106823e-03 9.84432834e-01 1.75529017e-01 -6.57407879e-02 + 1.42952270e-03 -1.75548706e-01 9.84469709e-01 3.97751007e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.74714223e-01 -8.82461041e-02 2.05292010e-01 -7.59260669e-02 + 5.05445047e-02 9.81975216e-01 1.82126131e-01 -6.30765562e-02 +-2.17663588e-01 -1.67144547e-01 9.61605565e-01 1.35296259e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.01303365e-01 -1.62948591e-01 4.01372645e-01 -1.49969134e-01 + 9.19155609e-02 9.77394242e-01 1.90399648e-01 -6.44812595e-02 +-4.23324667e-01 -1.34715452e-01 8.95906230e-01 3.81894526e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.K.txt b/data/sample/calibration/shelf/cam.poses.K.txt new file mode 100644 index 0000000..fcc115c --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.K.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 8.98476293e-01 2.45979981e-02 -4.38332397e-01 1.66427345e-01 +-1.08176815e-01 9.80048936e-01 -1.66738900e-01 7.41938015e-02 + 4.25485756e-01 1.97228351e-01 8.83211667e-01 3.97737409e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.75470532e-01 1.52229420e-02 -2.19603059e-01 8.11264007e-02 +-5.75697751e-02 9.80527707e-01 -1.87752861e-01 7.82270185e-02 + 2.12468733e-01 1.95789882e-01 9.57352265e-01 1.49537120e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99842415e-01 -2.33683749e-03 -1.75978376e-02 8.35634941e-03 +-8.89517266e-04 9.83458105e-01 -1.81133553e-01 7.10531378e-02 + 1.77300157e-02 1.81120663e-01 9.83301048e-01 6.74622495e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.66424202e-01 1.03804315e-02 2.56742104e-01 -1.10427523e-01 + 2.62933205e-02 9.89943595e-01 -1.38997626e-01 4.36554520e-02 +-2.55603057e-01 1.41081272e-01 9.56432513e-01 1.90488924e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.84244323e-01 3.58882966e-03 4.67010811e-01 -1.92327224e-01 + 6.08066304e-02 9.90573767e-01 -1.22744307e-01 4.15340885e-02 +-4.63049167e-01 1.36933310e-01 8.75691006e-01 4.85810902e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.03825708e-01 -8.91564275e-02 4.18509522e-01 -1.58141954e-01 + 8.87240064e-02 9.95844509e-01 2.05369301e-02 -3.18449986e-03 +-4.18601409e-01 1.85700362e-02 9.07980184e-01 3.47342047e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.75995040e-01 -4.50976396e-02 2.13072489e-01 -8.18185497e-02 + 4.59845206e-02 9.98941837e-01 7.94353471e-04 1.30189586e-03 +-2.12882847e-01 9.02275124e-03 9.77036071e-01 9.17945203e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.76712545e-01 5.23281497e-02 -2.08072990e-01 7.56139009e-02 +-5.09959009e-02 9.98629555e-01 1.17655865e-02 -2.31734640e-03 + 2.08403509e-01 -8.80726342e-04 9.78042536e-01 7.00097712e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.08913555e-01 1.05836711e-01 -4.03329568e-01 1.47095122e-01 +-9.96813598e-02 9.94357327e-01 3.62923314e-02 -7.53778302e-03 + 4.04894772e-01 7.21784779e-03 9.14334800e-01 2.84689736e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.08352673e-01 1.65568520e-01 -3.84034487e-01 1.43310303e-01 +-8.76199628e-02 9.73256877e-01 2.12352994e-01 -7.46024915e-02 + 4.08923176e-01 -1.59242322e-01 8.98567593e-01 3.28563560e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.76985306e-01 8.26098063e-02 -1.96660448e-01 7.15611851e-02 +-4.52750915e-02 9.81264213e-01 1.87271756e-01 -6.94223142e-02 + 2.08446343e-01 -1.74057934e-01 9.62420884e-01 1.18089181e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99983724e-01 -5.67516319e-03 5.86764712e-04 -1.68998450e-03 + 5.47876775e-03 9.83866620e-01 1.78819625e-01 -6.68154139e-02 +-1.59212876e-03 -1.78813499e-01 9.83881699e-01 4.50242538e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.75385732e-01 -8.74702398e-02 2.02414502e-01 -7.76070775e-02 + 5.13435690e-02 9.82817193e-01 1.77296936e-01 -6.39910958e-02 +-2.14444658e-01 -1.62540219e-01 9.63116902e-01 1.42047418e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.04327712e-01 -1.64007092e-01 3.94072410e-01 -1.49795738e-01 + 9.70259643e-02 9.78051747e-01 1.84392901e-01 -6.31526465e-02 +-4.15664952e-01 -1.28516354e-01 9.00392245e-01 3.77977366e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/shelf/cam.poses.L.txt b/data/sample/calibration/shelf/cam.poses.L.txt new file mode 100644 index 0000000..0b32c19 --- /dev/null +++ b/data/sample/calibration/shelf/cam.poses.L.txt @@ -0,0 +1,90 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000007 + 9.05729361e-01 3.71640543e-02 -4.22224062e-01 1.50664130e-01 +-9.89458566e-02 9.87164587e-01 -1.25362654e-01 5.08272995e-02 + 4.12145657e-01 1.55321958e-01 8.97781180e-01 3.67848556e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000007 + 9.75105142e-01 1.53755015e-02 -2.21209302e-01 7.87360629e-02 +-5.09686818e-02 9.86423716e-01 -1.56110363e-01 6.30757345e-02 + 2.15805827e-01 1.63498764e-01 9.62650507e-01 1.40821067e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000007 + 9.99971561e-01 4.89160349e-03 -5.74009802e-03 2.93507722e-04 +-5.87494064e-03 9.82494504e-01 -1.86198914e-01 7.28334622e-02 + 4.72880350e-03 1.86227341e-01 9.82495301e-01 7.86005286e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000007 + 9.77020644e-01 -2.65552934e-03 2.13128151e-01 -8.12200503e-02 + 4.28037465e-02 9.81996583e-01 -1.83985189e-01 7.14218509e-02 +-2.08802538e-01 1.88880012e-01 9.59544601e-01 1.92809200e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000007 + 8.96997234e-01 -6.92508740e-04 4.42035612e-01 -1.74316221e-01 + 9.44318385e-02 9.77213823e-01 -1.90094116e-01 8.10042269e-02 +-4.31831668e-01 2.12256132e-01 8.76623491e-01 5.18039841e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000007 + 9.08735660e-01 -1.01832289e-01 4.04758799e-01 -1.52992017e-01 + 1.04258030e-01 9.94419781e-01 1.61109611e-02 -3.71212699e-03 +-4.04140772e-01 2.75587501e-02 9.14281549e-01 3.27390579e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000007 + 9.77257602e-01 -5.39594989e-02 2.05075478e-01 -7.69960266e-02 + 5.55011486e-02 9.98457057e-01 -1.76850450e-03 2.04869377e-03 +-2.04663631e-01 1.31102090e-02 9.78744564e-01 9.72366584e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000007 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000007 + 9.73837407e-01 5.06621921e-02 -2.21526628e-01 8.07250962e-02 +-4.91563964e-02 9.98715244e-01 1.23089757e-02 -2.69237825e-03 + 2.21865620e-01 -1.09749026e-03 9.75076634e-01 7.32205005e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000007 + 9.02137279e-01 8.98455829e-02 -4.21990641e-01 1.54387755e-01 +-8.41537560e-02 9.95934373e-01 3.21383047e-02 -3.76576202e-03 + 4.23162469e-01 6.51893471e-03 9.06030368e-01 2.91911188e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000007 + 9.02797889e-01 1.51724526e-01 -4.02412277e-01 1.48177992e-01 +-7.46874412e-02 9.76795429e-01 2.00729858e-01 -6.53885489e-02 + 4.23530115e-01 -1.51163349e-01 8.93180768e-01 3.34037499e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000007 + 9.75124896e-01 7.65130667e-02 -2.08031701e-01 7.58481485e-02 +-3.87475766e-02 9.82923360e-01 1.79889670e-01 -6.08449786e-02 + 2.18243129e-01 -1.67354171e-01 9.61437735e-01 1.09856484e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000007 + 9.99952605e-01 -7.51840050e-03 -6.18561756e-03 1.84766041e-03 + 8.46373269e-03 9.85298522e-01 1.70631734e-01 -6.04134749e-02 + 4.81180212e-03 -1.70676000e-01 9.85315457e-01 4.32783936e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000007 + 9.75551298e-01 -9.03350727e-02 2.00347796e-01 -7.79602535e-02 + 5.61191340e-02 9.83789188e-01 1.70321683e-01 -6.27332116e-02 +-2.12486017e-01 -1.54914194e-01 9.64806346e-01 1.49302758e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000007 + 9.05154119e-01 -1.72260890e-01 3.88615758e-01 -1.49729015e-01 + 1.06971967e-01 9.77095785e-01 1.83958759e-01 -6.58104220e-02 +-4.11403719e-01 -1.24940037e-01 9.02849360e-01 3.92593000e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/calibration/tote/cam.poses.txt b/data/sample/calibration/tote/cam.poses.txt new file mode 100644 index 0000000..a8404ff --- /dev/null +++ b/data/sample/calibration/tote/cam.poses.txt @@ -0,0 +1,108 @@ +# Camera-to-camera extrinsic matrix (camera pose) from frame-000000 to frame-000000 + 1.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 1.00000000e+00 0.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000001 to frame-000000 + 9.63284338e-01 8.93708928e-03 -2.68334517e-01 9.94044247e-02 +-7.86956857e-03 9.99956264e-01 5.05364078e-03 -2.94007141e-04 + 2.68367946e-01 -2.75641614e-03 9.63312539e-01 1.25594272e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000002 to frame-000000 + 8.59164173e-01 1.77701037e-02 -5.11391384e-01 1.86398721e-01 +-1.66498836e-02 9.99838459e-01 6.77025413e-03 -6.12597017e-04 + 5.11429081e-01 2.69784721e-03 8.59321253e-01 6.15115392e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000003 to frame-000000 + 9.99991305e-01 2.73623363e-03 -3.14695596e-03 -8.51178056e-03 +-4.15288459e-03 7.22054641e-01 -6.91823568e-01 4.12727511e-01 + 3.79283241e-04 6.91830622e-01 7.22059725e-01 1.86636819e-01 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000004 to frame-000000 + 9.65441483e-01 -1.72879148e-01 -1.95027032e-01 4.79559861e-02 +-1.12601332e-02 7.19951759e-01 -6.93932759e-01 4.11531404e-01 + 2.60376559e-01 6.72147503e-01 6.93124651e-01 1.88702194e-01 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000005 to frame-000000 + 8.61019758e-01 -3.37413086e-01 -3.80522517e-01 9.67947428e-02 +-2.29136913e-02 7.21721027e-01 -6.91804685e-01 4.07980853e-01 + 5.08055055e-01 6.04376678e-01 6.13684684e-01 2.20289130e-01 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000006 to frame-000000 + 9.99947898e-01 1.35541701e-03 1.01175253e-02 -6.41927929e-03 +-1.39887054e-03 9.99989824e-01 4.28903766e-03 2.63011130e-01 +-1.01116089e-02 -4.30296730e-03 9.99939618e-01 4.34098204e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000007 to frame-000000 + 9.64926255e-01 9.86412262e-03 -2.62335703e-01 9.85789557e-02 +-8.64942638e-03 9.99945861e-01 5.78468719e-03 2.60634760e-01 + 2.62378561e-01 -3.31274320e-03 9.64959334e-01 1.56714296e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000008 to frame-000000 + 8.54577855e-01 1.51641158e-02 -5.19101859e-01 1.90802602e-01 +-1.18507462e-02 9.99882734e-01 9.69935332e-03 2.59575302e-01 + 5.19188068e-01 -2.13710819e-03 8.54657348e-01 6.92381128e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000009 to frame-000000 + 9.98264228e-01 4.92354633e-02 3.23171823e-02 -3.52978492e-02 +-1.29411951e-02 7.18701271e-01 -6.95198539e-01 6.71295234e-01 +-5.74548221e-02 6.93573609e-01 7.18090936e-01 1.94272808e-01 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000010 to frame-000000 + 9.74595989e-01 -1.38690287e-01 -1.75862624e-01 2.88503093e-02 +-2.23752803e-02 7.20985314e-01 -6.92589001e-01 6.69065270e-01 + 2.22849737e-01 6.78929437e-01 6.99566161e-01 1.94708557e-01 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000011 to frame-000000 + 8.79783967e-01 -3.08577643e-01 -3.61607536e-01 8.17258118e-02 +-2.74948113e-02 7.26376193e-01 -6.86747160e-01 6.64413163e-01 + 4.74577925e-01 6.14131471e-01 6.30569844e-01 2.21539215e-01 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000012 to frame-000000 + 9.98460507e-01 -3.36470069e-02 4.40964275e-02 -3.52967505e-02 + 6.44711625e-03 8.60010307e-01 5.10235931e-01 2.14944414e-01 +-5.50912941e-02 -5.09166132e-01 8.58903254e-01 8.68386692e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000013 to frame-000000 + 9.75786729e-01 1.07526752e-01 -1.90468518e-01 5.00691061e-02 + 2.37317947e-03 8.65559973e-01 5.00799661e-01 2.22406217e-01 + 2.18711287e-01 -4.89125679e-01 8.44346755e-01 8.80470267e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000014 to frame-000000 + 8.81193401e-01 2.39882741e-01 -4.07375086e-01 1.26237404e-01 +-6.57507889e-03 8.67838034e-01 4.96803699e-01 2.25633051e-01 + 4.72710227e-01 -4.35101618e-01 7.66310396e-01 1.24925280e-01 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000015 to frame-000000 + 9.99015992e-01 -4.42721155e-03 4.41298984e-02 -3.06546318e-02 + 8.40040390e-03 9.95882829e-01 -9.02597560e-02 5.84871704e-01 +-4.35486091e-02 9.05416486e-02 9.94940063e-01 1.82136442e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000016 to frame-000000 + 9.80145212e-01 -1.76466673e-02 -1.97494201e-01 5.34600796e-02 + 9.51529091e-04 9.96438925e-01 -8.43122961e-02 5.77709749e-01 + 1.98278740e-01 8.24503719e-02 9.76671632e-01 2.21101416e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-camera extrinsic matrix (camera pose) from frame-000017 to frame-000000 + 8.84437579e-01 -3.64866374e-02 -4.65229937e-01 1.52785287e-01 +-9.95536816e-03 9.95236594e-01 -9.69794393e-02 5.81643111e-01 + 4.66552312e-01 9.04037958e-02 8.79861406e-01 7.06553726e-02 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + diff --git a/data/sample/scene-0000/cam.info.txt b/data/sample/scene-0000/cam.info.txt new file mode 100755 index 0000000..5f0acb8 --- /dev/null +++ b/data/sample/scene-0000/cam.info.txt @@ -0,0 +1,115 @@ +# Environment: shelf +# Bin ID: B +# Objects: ["creativity_chenille_stems","i_am_a_bunny_book","kleenex_tissue_box","laugh_out_loud_joke_book"] + +# Color camera intrinsic matrix + 6.15195190e+02 0.00000000e+00 2.98442688e+02 + 0.00000000e+00 6.15195251e+02 2.47519196e+02 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Depth camera intrinsic matrix + 4.77584259e+02 0.00000000e+00 3.14188080e+02 + 0.00000000e+00 4.77584167e+02 2.45884354e+02 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Depth-to-color camera extrinsic matrix + 9.99997556e-01 1.12038769e-03 -1.88242714e-03 2.57000010e-02 +-1.10958668e-03 9.99982953e-01 5.72911976e-03 -7.33263500e-04 + 1.88881392e-03 -5.72701730e-03 9.99981821e-01 4.27287677e-03 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Bin-to-world transformation matrix + 9.99874572e-01 -1.58379443e-02 0.00000000e+00 1.47328990e+00 + 1.58379443e-02 9.99874572e-01 0.00000000e+00 -6.44825452e-03 + 0.00000000e+00 0.00000000e+00 1.00000000e+00 1.03730530e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000000 + 4.35946526e-01 3.73733732e-05 8.99972569e-01 1.17166368e+00 +-8.99972541e-01 -2.35527389e-04 4.35946522e-01 -1.63230649e-01 + 2.28260982e-04 -9.99999972e-01 -6.90423375e-05 1.15679915e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000001 + 2.32077070e-01 9.95129943e-06 9.72697401e-01 1.14566766e+00 +-9.72696947e-01 -9.64014128e-04 2.32076971e-01 -8.92647248e-02 + 9.40003504e-04 -9.99999535e-01 -2.14045975e-04 1.15633759e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000002 + 1.53726476e-02 -1.71137230e-03 9.99880369e-01 1.13649187e+00 +-9.99881770e-01 -3.84685960e-04 1.53720107e-02 -1.22663450e-02 + 3.58332706e-04 -9.99998462e-01 -1.71708361e-03 1.15502093e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000003 +-2.02105583e-01 -6.13757470e-04 9.79363547e-01 1.14335351e+00 +-9.79362592e-01 -1.40398068e-03 -2.02106266e-01 6.49973186e-02 + 1.49905173e-03 -9.99998826e-01 -3.17338773e-04 1.15597752e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000004 +-4.08931418e-01 -1.07051348e-03 9.12564490e-01 1.16781534e+00 +-9.12565109e-01 3.38506118e-04 -4.08931299e-01 1.39271616e-01 + 1.28857803e-04 -9.99999370e-01 -1.11533903e-03 1.15525260e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000005 +-4.09141298e-01 -1.60080291e-01 8.98319374e-01 1.17267698e+00 +-9.12468212e-01 6.93268067e-02 -4.03231393e-01 1.37168165e-01 + 2.27178536e-03 -9.84666489e-01 -1.74432637e-01 1.21742936e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000006 +-2.01617508e-01 -1.70584325e-01 9.64495396e-01 1.14899282e+00 +-9.79464102e-01 3.57904687e-02 -1.98416518e-01 6.42329659e-02 +-6.72994461e-04 -9.84692861e-01 -1.74297208e-01 1.21838674e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000007 + 1.62858052e-02 -1.73531591e-01 9.84693637e-01 1.14220022e+00 +-9.99866775e-01 -3.90777305e-03 1.58480901e-02 -1.13265838e-02 + 1.09781497e-03 -9.84820550e-01 -1.73572114e-01 1.21843008e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000008 + 2.32467868e-01 -1.67981400e-01 9.57987964e-01 1.15049211e+00 +-9.72604076e-01 -4.01892161e-02 2.28967548e-01 -8.73480890e-02 + 3.84959881e-05 -9.84970597e-01 -1.72722096e-01 1.21989622e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000009 + 4.37503635e-01 -1.55718683e-01 8.85630996e-01 1.17651872e+00 +-8.99216639e-01 -7.58412928e-02 4.30879954e-01 -1.59573303e-01 + 7.13408917e-05 -9.84885674e-01 -1.73205672e-01 1.21919407e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000010 + 4.36817301e-01 -3.08691497e-01 8.44926154e-01 1.19171207e+00 +-8.99549546e-01 -1.48727100e-01 4.10719934e-01 -1.53355789e-01 +-1.12233434e-03 -9.39462511e-01 -3.42649866e-01 1.27817403e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000011 + 2.31771055e-01 -3.32260810e-01 9.14267429e-01 1.16697345e+00 +-9.72770361e-01 -7.91045216e-02 2.17853851e-01 -8.40689998e-02 +-6.16093137e-05 -9.39864474e-01 -3.41547607e-01 1.27893275e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000012 + 1.56123268e-02 -3.42605021e-01 9.39349804e-01 1.15820572e+00 +-9.99878107e-01 -5.19669758e-03 1.47229595e-02 -1.13215035e-02 +-1.62642986e-04 -9.39465164e-01 -3.42644393e-01 1.27859912e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000013 +-2.01304797e-01 -3.35027864e-01 9.20452448e-01 1.16474898e+00 +-9.79527975e-01 6.99560264e-02 -1.88762019e-01 6.16719046e-02 +-1.15065993e-03 -9.39607623e-01 -3.42251649e-01 1.27909965e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 + +# Camera-to-world extrinsic matrix (camera pose) for frame-000014 +-4.08026181e-01 -3.11864392e-01 8.58053167e-01 1.18650572e+00 +-9.12970203e-01 1.39160983e-01 -3.83561768e-01 1.31465255e-01 + 2.11735735e-04 -9.39880217e-01 -3.41504221e-01 1.27962559e+00 + 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00 diff --git a/data/sample/scene-0000/frame-000000.color.png b/data/sample/scene-0000/frame-000000.color.png new file mode 100755 index 0000000..66ff737 Binary files /dev/null and b/data/sample/scene-0000/frame-000000.color.png differ diff --git a/data/sample/scene-0000/frame-000000.depth.png b/data/sample/scene-0000/frame-000000.depth.png new file mode 100755 index 0000000..9e55675 Binary files /dev/null and b/data/sample/scene-0000/frame-000000.depth.png differ diff --git a/data/sample/scene-0000/frame-000001.color.png b/data/sample/scene-0000/frame-000001.color.png new file mode 100755 index 0000000..9c6e5a9 Binary files /dev/null and b/data/sample/scene-0000/frame-000001.color.png differ diff --git a/data/sample/scene-0000/frame-000001.depth.png b/data/sample/scene-0000/frame-000001.depth.png new file mode 100755 index 0000000..f2b0e38 Binary files /dev/null and b/data/sample/scene-0000/frame-000001.depth.png differ diff --git a/data/sample/scene-0000/frame-000002.color.png b/data/sample/scene-0000/frame-000002.color.png new file mode 100755 index 0000000..e4e8763 Binary files /dev/null and b/data/sample/scene-0000/frame-000002.color.png differ diff --git a/data/sample/scene-0000/frame-000002.depth.png b/data/sample/scene-0000/frame-000002.depth.png new file mode 100755 index 0000000..15d4392 Binary files /dev/null and b/data/sample/scene-0000/frame-000002.depth.png differ diff --git a/data/sample/scene-0000/frame-000003.color.png b/data/sample/scene-0000/frame-000003.color.png new file mode 100755 index 0000000..98eb99e Binary files /dev/null and b/data/sample/scene-0000/frame-000003.color.png differ diff --git a/data/sample/scene-0000/frame-000003.depth.png b/data/sample/scene-0000/frame-000003.depth.png new file mode 100755 index 0000000..6239836 Binary files /dev/null and b/data/sample/scene-0000/frame-000003.depth.png differ diff --git a/data/sample/scene-0000/frame-000004.color.png b/data/sample/scene-0000/frame-000004.color.png new file mode 100755 index 0000000..cf99bc8 Binary files /dev/null and b/data/sample/scene-0000/frame-000004.color.png differ diff --git a/data/sample/scene-0000/frame-000004.depth.png b/data/sample/scene-0000/frame-000004.depth.png new file mode 100755 index 0000000..e555953 Binary files /dev/null and b/data/sample/scene-0000/frame-000004.depth.png differ diff --git a/data/sample/scene-0000/frame-000005.color.png b/data/sample/scene-0000/frame-000005.color.png new file mode 100755 index 0000000..ddfb117 Binary files /dev/null and b/data/sample/scene-0000/frame-000005.color.png differ diff --git a/data/sample/scene-0000/frame-000005.depth.png b/data/sample/scene-0000/frame-000005.depth.png new file mode 100755 index 0000000..95f0cd5 Binary files /dev/null and b/data/sample/scene-0000/frame-000005.depth.png differ diff --git a/data/sample/scene-0000/frame-000006.color.png b/data/sample/scene-0000/frame-000006.color.png new file mode 100755 index 0000000..b2e2488 Binary files /dev/null and b/data/sample/scene-0000/frame-000006.color.png differ diff --git a/data/sample/scene-0000/frame-000006.depth.png b/data/sample/scene-0000/frame-000006.depth.png new file mode 100755 index 0000000..d17a3c3 Binary files /dev/null and b/data/sample/scene-0000/frame-000006.depth.png differ diff --git a/data/sample/scene-0000/frame-000007.color.png b/data/sample/scene-0000/frame-000007.color.png new file mode 100755 index 0000000..b4d3174 Binary files /dev/null and b/data/sample/scene-0000/frame-000007.color.png differ diff --git a/data/sample/scene-0000/frame-000007.depth.png b/data/sample/scene-0000/frame-000007.depth.png new file mode 100755 index 0000000..095dfbe Binary files /dev/null and b/data/sample/scene-0000/frame-000007.depth.png differ diff --git a/data/sample/scene-0000/frame-000008.color.png b/data/sample/scene-0000/frame-000008.color.png new file mode 100755 index 0000000..1a57ca3 Binary files /dev/null and b/data/sample/scene-0000/frame-000008.color.png differ diff --git a/data/sample/scene-0000/frame-000008.depth.png b/data/sample/scene-0000/frame-000008.depth.png new file mode 100755 index 0000000..1782980 Binary files /dev/null and b/data/sample/scene-0000/frame-000008.depth.png differ diff --git a/data/sample/scene-0000/frame-000009.color.png b/data/sample/scene-0000/frame-000009.color.png new file mode 100755 index 0000000..06bf0e9 Binary files /dev/null and b/data/sample/scene-0000/frame-000009.color.png differ diff --git a/data/sample/scene-0000/frame-000009.depth.png b/data/sample/scene-0000/frame-000009.depth.png new file mode 100755 index 0000000..7011028 Binary files /dev/null and b/data/sample/scene-0000/frame-000009.depth.png differ diff --git a/data/sample/scene-0000/frame-000010.color.png b/data/sample/scene-0000/frame-000010.color.png new file mode 100755 index 0000000..8afbb2a Binary files /dev/null and b/data/sample/scene-0000/frame-000010.color.png differ diff --git a/data/sample/scene-0000/frame-000010.depth.png b/data/sample/scene-0000/frame-000010.depth.png new file mode 100755 index 0000000..82541b0 Binary files /dev/null and b/data/sample/scene-0000/frame-000010.depth.png differ diff --git a/data/sample/scene-0000/frame-000011.color.png b/data/sample/scene-0000/frame-000011.color.png new file mode 100755 index 0000000..dd27534 Binary files /dev/null and b/data/sample/scene-0000/frame-000011.color.png differ diff --git a/data/sample/scene-0000/frame-000011.depth.png b/data/sample/scene-0000/frame-000011.depth.png new file mode 100755 index 0000000..f5ca1f1 Binary files /dev/null and b/data/sample/scene-0000/frame-000011.depth.png differ diff --git a/data/sample/scene-0000/frame-000012.color.png b/data/sample/scene-0000/frame-000012.color.png new file mode 100755 index 0000000..99a023a Binary files /dev/null and b/data/sample/scene-0000/frame-000012.color.png differ diff --git a/data/sample/scene-0000/frame-000012.depth.png b/data/sample/scene-0000/frame-000012.depth.png new file mode 100755 index 0000000..d7a64b7 Binary files /dev/null and b/data/sample/scene-0000/frame-000012.depth.png differ diff --git a/data/sample/scene-0000/frame-000013.color.png b/data/sample/scene-0000/frame-000013.color.png new file mode 100755 index 0000000..b18b1fb Binary files /dev/null and b/data/sample/scene-0000/frame-000013.color.png differ diff --git a/data/sample/scene-0000/frame-000013.depth.png b/data/sample/scene-0000/frame-000013.depth.png new file mode 100755 index 0000000..ba1f131 Binary files /dev/null and b/data/sample/scene-0000/frame-000013.depth.png differ diff --git a/data/sample/scene-0000/frame-000014.color.png b/data/sample/scene-0000/frame-000014.color.png new file mode 100755 index 0000000..c7a6fe5 Binary files /dev/null and b/data/sample/scene-0000/frame-000014.color.png differ diff --git a/data/sample/scene-0000/frame-000014.depth.png b/data/sample/scene-0000/frame-000014.depth.png new file mode 100755 index 0000000..342d42d Binary files /dev/null and b/data/sample/scene-0000/frame-000014.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000000.depth.png b/data/sample/scene-0000/raw/frame-000000.depth.png new file mode 100755 index 0000000..a36fa6e Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000000.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000001.depth.png b/data/sample/scene-0000/raw/frame-000001.depth.png new file mode 100755 index 0000000..f59a7e1 Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000001.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000002.depth.png b/data/sample/scene-0000/raw/frame-000002.depth.png new file mode 100755 index 0000000..12c5e9e Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000002.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000003.depth.png b/data/sample/scene-0000/raw/frame-000003.depth.png new file mode 100755 index 0000000..d75aa7a Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000003.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000004.depth.png b/data/sample/scene-0000/raw/frame-000004.depth.png new file mode 100755 index 0000000..97abb31 Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000004.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000005.depth.png b/data/sample/scene-0000/raw/frame-000005.depth.png new file mode 100755 index 0000000..96995d8 Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000005.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000006.depth.png b/data/sample/scene-0000/raw/frame-000006.depth.png new file mode 100755 index 0000000..2b48a00 Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000006.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000007.depth.png b/data/sample/scene-0000/raw/frame-000007.depth.png new file mode 100755 index 0000000..a489c30 Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000007.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000008.depth.png b/data/sample/scene-0000/raw/frame-000008.depth.png new file mode 100755 index 0000000..8bca0cf Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000008.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000009.depth.png b/data/sample/scene-0000/raw/frame-000009.depth.png new file mode 100755 index 0000000..684d3ba Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000009.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000010.depth.png b/data/sample/scene-0000/raw/frame-000010.depth.png new file mode 100755 index 0000000..1db319d Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000010.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000011.depth.png b/data/sample/scene-0000/raw/frame-000011.depth.png new file mode 100755 index 0000000..382b72f Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000011.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000012.depth.png b/data/sample/scene-0000/raw/frame-000012.depth.png new file mode 100755 index 0000000..aa10ab0 Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000012.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000013.depth.png b/data/sample/scene-0000/raw/frame-000013.depth.png new file mode 100755 index 0000000..ecb099f Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000013.depth.png differ diff --git a/data/sample/scene-0000/raw/frame-000014.depth.png b/data/sample/scene-0000/raw/frame-000014.depth.png new file mode 100755 index 0000000..cf294f4 Binary files /dev/null and b/data/sample/scene-0000/raw/frame-000014.depth.png differ diff --git a/data/sample/scene-0000/segm/frame-000000.segm.png b/data/sample/scene-0000/segm/frame-000000.segm.png new file mode 100644 index 0000000..389c821 Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000000.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000001.segm.png b/data/sample/scene-0000/segm/frame-000001.segm.png new file mode 100644 index 0000000..d235f55 Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000001.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000002.segm.png b/data/sample/scene-0000/segm/frame-000002.segm.png new file mode 100644 index 0000000..61eac9b Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000002.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000003.segm.png b/data/sample/scene-0000/segm/frame-000003.segm.png new file mode 100644 index 0000000..826cbee Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000003.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000004.segm.png b/data/sample/scene-0000/segm/frame-000004.segm.png new file mode 100644 index 0000000..a1ef66f Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000004.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000005.segm.png b/data/sample/scene-0000/segm/frame-000005.segm.png new file mode 100644 index 0000000..a0202fe Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000005.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000006.segm.png b/data/sample/scene-0000/segm/frame-000006.segm.png new file mode 100644 index 0000000..ebb3e49 Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000006.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000007.segm.png b/data/sample/scene-0000/segm/frame-000007.segm.png new file mode 100644 index 0000000..87f1532 Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000007.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000008.segm.png b/data/sample/scene-0000/segm/frame-000008.segm.png new file mode 100644 index 0000000..c8bc061 Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000008.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000009.segm.png b/data/sample/scene-0000/segm/frame-000009.segm.png new file mode 100644 index 0000000..8ce5f21 Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000009.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000010.segm.png b/data/sample/scene-0000/segm/frame-000010.segm.png new file mode 100644 index 0000000..fa4fd5a Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000010.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000011.segm.png b/data/sample/scene-0000/segm/frame-000011.segm.png new file mode 100644 index 0000000..382f9ac Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000011.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000012.segm.png b/data/sample/scene-0000/segm/frame-000012.segm.png new file mode 100644 index 0000000..91ba0cc Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000012.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000013.segm.png b/data/sample/scene-0000/segm/frame-000013.segm.png new file mode 100644 index 0000000..9fc5c9f Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000013.segm.png differ diff --git a/data/sample/scene-0000/segm/frame-000014.segm.png b/data/sample/scene-0000/segm/frame-000014.segm.png new file mode 100644 index 0000000..7abede7 Binary files /dev/null and b/data/sample/scene-0000/segm/frame-000014.segm.png differ diff --git a/evaluation/runEval.m b/evaluation/runEval.m index 04b8ab7..b4928f2 100644 --- a/evaluation/runEval.m +++ b/evaluation/runEval.m @@ -36,9 +36,9 @@ load(fullfile(benchmarkPath,'scenes.mat')); % Get object names -load('objectNames'); +load('objectNames.mat'); -% Get set of colors for visualizations +% Get set of colors for visualizations (from vis-utils) load('colorPalette.mat'); % Loop through benchmark scenes diff --git a/rgbd-utils/matlab/loadCalib.m b/rgbd-utils/matlab/loadCalib.m index a675553..33428a8 100644 --- a/rgbd-utils/matlab/loadCalib.m +++ b/rgbd-utils/matlab/loadCalib.m @@ -6,8 +6,8 @@ % Input: % calibDir - file path to the folder that contains the set of relative % camera-to-camera poses generated from getCalib -% seqData - data structure holding the contents (frames and camera -% information) of a captured RGB-D sequence +% sceneData - data structure holding the contents (frames and camera +% information) of a captured RGB-D scene % Output: % sceneData - scene data with calibrated camera-to-world matrices % diff --git a/ros-packages/catkin_ws/src/marvin_convnet/models/competition/download_weights.sh b/ros-packages/catkin_ws/src/marvin_convnet/models/competition/download_weights.sh new file mode 100644 index 0000000..63e754d --- /dev/null +++ b/ros-packages/catkin_ws/src/marvin_convnet/models/competition/download_weights.sh @@ -0,0 +1,3 @@ +cd "$( dirname "${BASH_SOURCE[0]}" )" +curl -O http://vision.princeton.edu/projects/2016/apc/downloads/weights_shelf.marvin +curl -O http://vision.princeton.edu/projects/2016/apc/downloads/weights_tote.marvin diff --git a/ros-packages/catkin_ws/src/marvin_convnet/models/competition/net.json b/ros-packages/catkin_ws/src/marvin_convnet/models/competition/net.json new file mode 100755 index 0000000..2585672 --- /dev/null +++ b/ros-packages/catkin_ws/src/marvin_convnet/models/competition/net.json @@ -0,0 +1,492 @@ +{ + "train": + { + "path": "apc_", + "solver": "SGD", + "regularizer": "L2", + "momentum": 0.99, + "weight_decay": 0.0005, + "base_lr": 0.01, + "lr_policy": "LR_inv", + "lr_gamma": 0.0001, + "lr_power": 0.75, + "max_iter": 1000000, + "train_iter": 1, + "snapshot_iter": 100, + "display_iter": 1, + "test_iter": 10, + "test_interval": 100, + "debug_mode": false, + "GPU_solver": 0, + "GPU": [0] + }, + "test": + { + "debug_mode": false, + "GPU": 0 + }, + "layers":[ + { + "type": "PlaceHolderData", + "name": "dataTest", + "phase": "Testing", + "dim": [1,3,480,640], + "out": ["data_RGB"] + }, + { + "type": "PlaceHolderData", + "name": "dataTest", + "phase": "Testing", + "dim": [1,3,480,640], + "out": ["data_HHA"] + }, + { + "in": ["data_RGB"], + "type": "Convolution", + "name": "conv1_1", + "num_output": 64, + "window": [3,3], + "padding": [186,186], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv1_1"] + }, + { + "in": ["conv1_1"], + "type": "Activation", + "name": "relu1_1", + "mode": "ReLU", + "out": ["conv1_1"] + }, + { + "in": ["conv1_1"], + "type": "Convolution", + "name": "conv1_2", + "num_output": 64, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv1_2"] + }, + { + "in": ["conv1_2"], + "type": "Activation", + "name": "relu1_2", + "mode": "ReLU", + "out": ["conv1_2"] + }, + { + "in": ["conv1_2"], + "type": "Pooling", + "name": "pool1", + "mode": "max", + "window": [2,2], + "padding": [0,0], + "stride": [2,2], + "out": ["pool1"] + }, + { + "in": ["pool1"], + "type": "Convolution", + "name": "conv2_1", + "num_output": 128, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv2_1"] + }, + { + "in": ["conv2_1"], + "type": "Activation", + "name": "relu2_1", + "mode": "ReLU", + "out": ["conv2_1"] + }, + { + "in": ["conv2_1"], + "type": "Convolution", + "name": "conv2_2", + "num_output": 128, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv2_2"] + }, + { + "in": ["conv2_2"], + "type": "Activation", + "name": "relu2_2", + "mode": "ReLU", + "out": ["conv2_2"] + }, + { + "in": ["conv2_2"], + "type": "Pooling", + "name": "pool2", + "mode": "max", + "window": [2,2], + "padding": [0,0], + "stride": [2,2], + "out": ["pool2"] + }, + { + "in": ["pool2"], + "type": "Convolution", + "name": "conv3_1", + "num_output": 256, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv3_1"] + }, + { + "in": ["conv3_1"], + "type": "Activation", + "name": "relu3_1", + "mode": "ReLU", + "out": ["conv3_1"] + }, + { + "in": ["conv3_1"], + "type": "Convolution", + "name": "conv3_2", + "num_output": 256, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv3_2"] + }, + { + "in": ["conv3_2"], + "type": "Activation", + "name": "relu3_2", + "mode": "ReLU", + "out": ["conv3_2"] + }, + { + "in": ["conv3_2"], + "type": "Convolution", + "name": "conv3_3", + "num_output": 256, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv3_3"] + }, + { + "in": ["conv3_3"], + "type": "Activation", + "name": "relu3_3", + "mode": "ReLU", + "out": ["conv3_3"] + }, + { + "in": ["conv3_3"], + "type": "Pooling", + "name": "pool3", + "mode": "max", + "window": [2,2], + "padding": [0,0], + "stride": [2,2], + "out": ["pool3"] + }, + { + "in": ["pool3"], + "type": "Convolution", + "name": "conv4_1", + "num_output": 512, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv4_1"] + }, + { + "in": ["conv4_1"], + "type": "Activation", + "name": "relu4_1", + "mode": "ReLU", + "out": ["conv4_1"] + }, + { + "in": ["conv4_1"], + "type": "Convolution", + "name": "conv4_2", + "num_output": 512, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv4_2"] + }, + { + "in": ["conv4_2"], + "type": "Activation", + "name": "relu4_2", + "mode": "ReLU", + "out": ["conv4_2"] + }, + { + "in": ["conv4_2"], + "type": "Convolution", + "name": "conv4_3", + "num_output": 512, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv4_3"] + }, + { + "in": ["conv4_3"], + "type": "Activation", + "name": "relu4_3", + "mode": "ReLU", + "out": ["conv4_3"] + }, + { + "in": ["conv4_3"], + "type": "Pooling", + "name": "pool4", + "mode": "max", + "window": [2,2], + "padding": [0,0], + "stride": [2,2], + "out": ["pool4"] + }, + { + "in": ["pool4"], + "type": "Convolution", + "name": "conv5_1", + "num_output": 512, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv5_1"] + }, + { + "in": ["conv5_1"], + "type": "Activation", + "name": "relu5_1", + "mode": "ReLU", + "out": ["conv5_1"] + }, + { + "in": ["conv5_1"], + "type": "Convolution", + "name": "conv5_2", + "num_output": 512, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv5_2"] + }, + { + "in": ["conv5_2"], + "type": "Activation", + "name": "relu5_2", + "mode": "ReLU", + "out": ["conv5_2"] + }, + { + "in": ["conv5_2"], + "type": "Convolution", + "name": "conv5_3", + "num_output": 512, + "window": [3,3], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["conv5_3"] + }, + { + "in": ["conv5_3"], + "type": "Activation", + "name": "relu5_3", + "mode": "ReLU", + "out": ["conv5_3"] + }, + { + "in": ["conv5_3"], + "type": "Pooling", + "name": "pool5", + "mode": "max", + "window": [2,2], + "padding": [0,0], + "stride": [2,2], + "out": ["pool5"] + }, + { + "in": ["pool5"], + "type": "Convolution", + "name": "fc6", + "num_output": 4096, + "window": [7,7], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "train_me": false, + "out": ["fc6"] + }, + { + "in": ["fc6"], + "type": "Activation", + "name": "relu6", + "mode": "ReLU", + "out": ["fc6"] + }, + { + "in": ["fc6"], + "type": "Convolution", + "name": "fc7", + "num_output": 4096, + "window": [1,1], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "out": ["fc7"] + }, + { + "in": ["fc7"], + "type": "Activation", + "name": "relu7", + "mode": "ReLU", + "out": ["fc7"] + }, + { + "in": ["fc7"], + "type": "Convolution", + "name": "score", + "num_output": 40, + "window": [1,1], + "padding": [0,0], + "stride": [1,1], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "out": ["score"] + }, + { + "in": ["score"], + "type": "Deconvolution", + "name": "deconv", + "num_output": 40, + "window": [64,64], + "padding": [16,16], + "stride": [32,32], + "upscale": [1,1], + "weight_lr_mult": 1.0, + "weight_filler": "Xavier", + "bias_lr_mult": 2.0, + "bias_filler": "Constant", + "bias_filler_param": 0.0, + "out": ["prob"] + }, + { + "in": ["prob"], + "type": "Softmax", + "stable_gradient": true, + "name": "prob", + "out": ["prob"] + } + ] +} diff --git a/ros-packages/catkin_ws/src/marvin_convnet/src/detect.cu b/ros-packages/catkin_ws/src/marvin_convnet/src/detect.cu index f73d6fc..42d7bd6 100755 --- a/ros-packages/catkin_ws/src/marvin_convnet/src/detect.cu +++ b/ros-packages/catkin_ws/src/marvin_convnet/src/detect.cu @@ -24,11 +24,9 @@ std::string tote_net_weights_filename = "/home/andyz/apc/toolbox/ros-packages/ca // Service modes and names std::string service_name; -std::string camera_service_name; // Directory to read/write all RGB-D files and response maps std::string read_directory; -std::string net_directory; // Global buffers for sensor data retrieval int frame_width = 640; @@ -158,9 +156,7 @@ int main(int argc, char **argv) { // Get service parameters priv_nh.param("service_name", service_name, std::string("marvin_convnet")); - priv_nh.param("camera_service_name", camera_service_name, std::string("/realsense_camera")); priv_nh.param("read_directory", read_directory, std::string("")); - priv_nh.param("net_directory", net_directory, std::string("")); // Assert parameters and create folder to save segmentation masks assert(!read_directory.empty()); diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/adjustShelfPose.m b/ros-packages/catkin_ws/src/pose_estimation/src/adjustShelfPose.m deleted file mode 100644 index 834c75f..0000000 --- a/ros-packages/catkin_ws/src/pose_estimation/src/adjustShelfPose.m +++ /dev/null @@ -1,148 +0,0 @@ -clear all; close all; - -alignPath = '/home/mcube/apcdata/princeton_data/raw/shelf_align'; -savetoPath = '/home/mcube/apcdata/princeton_data/raw/shelf_align/poses'; - -% Align all frames to the middle frame (000007) - -% run('lib/vlfeat/toolbox/vl_setup.m'); -% alignDir = dir(alignPath); -addpath(fullfile('lib/peter')); -addpath(fullfile('lib/estimateRigidTransform')); -addpath(fullfile('lib/sfm')); - -alignDir = dir(fullfile(alignPath,'00*')); - -for seqIdx = 1:length(alignDir) - - seqPath = fullfile(alignPath,alignDir(seqIdx).name); - seqSaveToPath = fullfile(savetoPath,alignDir(seqIdx).name); - mkdir(seqSaveToPath); - - % Read RGB-D frames - allCamPts = []; - allColorPts = []; - depthFiles = dir(fullfile(seqPath,'*.regdepth.png')); - colorFiles = dir(fullfile(seqPath,'*.color.png')); - extCam2WorldFiles = dir(fullfile(seqPath,'*.pose_camera_map.txt')); - colorK = dlmread(fullfile(seqPath,'color_intrinsics.K.txt')); - extWorld2Bin = inv(dlmread(fullfile(seqPath,sprintf('pose_bin%d_map.txt',seqIdx-1)))); - - allColorIm = {}; - allDepthIm = {}; - allExtCam2World = {}; - allCamPts = {}; - allColorPts = {}; - allSURF2D = {}; - allSURF3D = {}; - allSURFDesc = {}; - % newExt = {}; - for frameIdx = [8 7 6 9 10 3 2 1 4 5 13 12 11 14 15] %1:length(extCam2WorldFiles) - colorIm = imread(fullfile(seqPath,colorFiles(frameIdx).name)); - depthIm = double(imread(fullfile(seqPath,depthFiles(frameIdx).name)))./10000; -% extCam2World = dlmread(fullfile(seqPath,extCam2WorldFiles(frameIdx).name)); - allColorIm{frameIdx} = colorIm; - allDepthIm{frameIdx} = depthIm; - - % Project depth into camera space - [pixX,pixY] = meshgrid(1:640,1:480); - camX = (pixX-colorK(1,3)).*allDepthIm{frameIdx}/colorK(1,1); - camY = (pixY-colorK(2,3)).*allDepthIm{frameIdx}/colorK(2,2); - camZ = allDepthIm{frameIdx}; - - % Only use points with valid depth - validDepth = find(camZ > 0); - camPts = [camX(validDepth),camY(validDepth),camZ(validDepth)]'; - allCamPts{frameIdx} = camPts; - % allCamPts{frameIdx} = extCam2World(1:3,1:3) * camPts + repmat(extCam2World(1:3,4),1,size(camPts,2)); - - % Get vertex colors - colorR = colorIm(:,:,1); - colorG = colorIm(:,:,2); - colorB = colorIm(:,:,3); - allColorPts{frameIdx} = [colorR(validDepth),colorG(validDepth),colorB(validDepth)]'; - - % Extract SURF features of current frame - currFrameSURF = detectSURFFeatures(rgb2gray(colorIm)); - [currFrameDesc,currFrameSURF] = extractFeatures(rgb2gray(colorIm), currFrameSURF); - - % Get SURF points in 3D of current frame - currSURFpts3D = []; - depthIm = allDepthIm{frameIdx}; - for SURFIdx = 1:length(currFrameSURF) - currSURF2D = round(currFrameSURF(SURFIdx).Location'); - currSURF3D = zeros(3,1); - currSURF3D(1) = (currSURF2D(1)-colorK(1,3)).*depthIm(currSURF2D(2),currSURF2D(1))/colorK(1,1); - currSURF3D(2) = (currSURF2D(2)-colorK(2,3)).*depthIm(currSURF2D(2),currSURF2D(1))/colorK(2,2); - currSURF3D(3) = depthIm(currSURF2D(2),currSURF2D(1)); - % currSURF3D = extCam2World(1:3,1:3) * currSURF3D + repmat(extCam2World(1:3,4),1,size(currSURF3D,2)); - currSURFpts3D = [currSURFpts3D currSURF3D]; - end - - % Only use SURF points with valid depth - validDepth = find(currSURFpts3D(3,:) > 0.15); - currFrameSURF = currFrameSURF(validDepth); - currFrameDesc = currFrameDesc(validDepth,:); - currSURFpts3D = currSURFpts3D(:,validDepth); - allSURF2D{frameIdx} = currFrameSURF; - allSURFDesc{frameIdx} = currFrameDesc; - allSURF3D{frameIdx} = currSURFpts3D; - - % Get index of previous frame - if frameIdx == 8 -% allExtCam2World{frameIdx} = extCam2World; - allExtCam2World{frameIdx} = eye(4); - dlmwrite(fullfile(seqSaveToPath,extCam2WorldFiles(frameIdx).name),allExtCam2World{frameIdx},'delimiter',' '); - continue; - elseif mod(frameIdx,5) == 3 - prevFrameIdx = 8; - elseif mod(frameIdx,5) == 2 - prevFrameIdx = frameIdx + 1; - elseif mod(frameIdx,5) == 1 - prevFrameIdx = frameIdx + 1; - elseif mod(frameIdx,5) == 4 - prevFrameIdx = frameIdx - 1; - elseif mod(frameIdx,5) == 0 - prevFrameIdx = frameIdx - 1; - end - - % Get SURF features of previous frame - prevFrameSURF = allSURF2D{prevFrameIdx}; - prevFrameDesc = allSURFDesc{prevFrameIdx}; - prevSURFpts3D = allSURF3D{prevFrameIdx}; - - % Match SURF features - matchInd = matchFeatures(prevFrameDesc,currFrameDesc); - prevMatchSURF = prevFrameSURF(matchInd(:,1)); - currMatchSURF = currFrameSURF(matchInd(:,2)); - prevMatchPts3D = prevSURFpts3D(:,matchInd(:,1)); - currMatchPts3D = currSURFpts3D(:,matchInd(:,2)); - % showMatchedFeatures(allColorIm{prevFrameIdx},allColorIm{frameIdx},prevMatchSURF,currMatchSURF); - - [initRt, ransacInliers] = ransacfitRt([prevMatchPts3D;currMatchPts3D], 0.005,0); - fprintf('RANSAC inliers: %d/%d\n',size(ransacInliers,2),size(currMatchPts3D,2)); - - allExtCam2World{frameIdx} = allExtCam2World{prevFrameIdx} * [initRt; [0 0 0 1]]; - currExtCam2World = allExtCam2World{frameIdx}; - -% currCamPts = currExtCam2World(1:3,1:3) * allCamPts{frameIdx} + repmat(currExtCam2World(1:3,4),1,size(allCamPts{frameIdx},2)); -% currCloud = pointCloud(currCamPts','Color',allColorPts{frameIdx}'); -% pcwrite(currCloud,sprintf('%d',frameIdx),'PLYFormat','binary'); - - dlmwrite(fullfile(seqSaveToPath,extCam2WorldFiles(frameIdx).name),currExtCam2World,'delimiter',' '); - - end - -end - - - - - - - - - - - - diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/adjustTotePose.m b/ros-packages/catkin_ws/src/pose_estimation/src/adjustTotePose.m deleted file mode 100644 index 14c37ae..0000000 --- a/ros-packages/catkin_ws/src/pose_estimation/src/adjustTotePose.m +++ /dev/null @@ -1,137 +0,0 @@ -clear all; close all; - -% alignPath = 'C:/Users/andyz/Documents/apcdata/tote_align/000000'; -% savetoPath = 'C:/Users/andyz/Documents/apcdata/tote_align/poses'; -% alignPath = '/home/mcube/apcdata/princeton_data/raw/tote_align/000000'; -% savetoPath = '/home/mcube/apcdata/princeton_data/raw/tote_align/poses'; -% alignPath = '/home/mcube/apcdata/princeton_data/raw/tote_test6/calib'; -% savetoPath = '/home/mcube/apcdata/princeton_data/raw/tote_test6/poses'; -alignPath = '/home/mcube/apcdata/princeton_data/raw/tote_align/calib'; -savetoPath = '/home/mcube/apcdata/princeton_data/raw/tote_align/poses'; - - -% run('lib/vlfeat/toolbox/vl_setup.m'); -% alignDir = dir(alignPath); -addpath(fullfile('lib/peter')); -addpath(fullfile('lib/estimateRigidTransform')); -addpath(fullfile('lib/sfm')); -mkdir(savetoPath); - -% Read RGB-D frames -allCamPts = []; -allColorPts = []; -depthFiles = dir(fullfile(alignPath,'*.regdepth.png')); -colorFiles = dir(fullfile(alignPath,'*.color.png')); -extCam2WorldFiles = dir(fullfile(alignPath,'*.pose_camera_map.txt')); -colorK = dlmread(fullfile(alignPath,'color_intrinsics.K.txt')); -extWorld2Bin = inv(dlmread(fullfile(alignPath,'pose_bin-1_map.txt'))); - -allColorIm = {}; -allDepthIm = {}; -allExtCam2World = {}; -allCamPts = {}; -allColorPts = {}; -allSURF2D = {}; -allSURF3D = {}; -allSURFDesc = {}; -% newExt = {}; -for frameIdx = 1:length(extCam2WorldFiles) - colorIm = imread(fullfile(alignPath,colorFiles(frameIdx).name)); - depthIm = double(imread(fullfile(alignPath,depthFiles(frameIdx).name)))./10000; - extCam2World = dlmread(fullfile(alignPath,extCam2WorldFiles(frameIdx).name)); - allColorIm{frameIdx} = colorIm; - allDepthIm{frameIdx} = depthIm; - - % Project depth into camera space - [pixX,pixY] = meshgrid(1:640,1:480); - camX = (pixX-colorK(1,3)).*allDepthIm{frameIdx}/colorK(1,1); - camY = (pixY-colorK(2,3)).*allDepthIm{frameIdx}/colorK(2,2); - camZ = allDepthIm{frameIdx}; - - % Only use points with valid depth - validDepth = find(camZ > 0); - camPts = [camX(validDepth),camY(validDepth),camZ(validDepth)]'; - allCamPts{frameIdx} = camPts; -% allCamPts{frameIdx} = extCam2World(1:3,1:3) * camPts + repmat(extCam2World(1:3,4),1,size(camPts,2)); - - % Get vertex colors - colorR = colorIm(:,:,1); - colorG = colorIm(:,:,2); - colorB = colorIm(:,:,3); - allColorPts{frameIdx} = [colorR(validDepth),colorG(validDepth),colorB(validDepth)]'; - - % Extract SURF features of current frame - currFrameSURF = detectSURFFeatures(rgb2gray(colorIm)); - [currFrameDesc,currFrameSURF] = extractFeatures(rgb2gray(colorIm), currFrameSURF); - - % Get SURF points in 3D of current frame - currSURFpts3D = []; - depthIm = allDepthIm{frameIdx}; - for SURFIdx = 1:length(currFrameSURF) - currSURF2D = round(currFrameSURF(SURFIdx).Location'); - currSURF3D = zeros(3,1); - currSURF3D(1) = (currSURF2D(1)-colorK(1,3)).*depthIm(currSURF2D(2),currSURF2D(1))/colorK(1,1); - currSURF3D(2) = (currSURF2D(2)-colorK(2,3)).*depthIm(currSURF2D(2),currSURF2D(1))/colorK(2,2); - currSURF3D(3) = depthIm(currSURF2D(2),currSURF2D(1)); -% currSURF3D = extCam2World(1:3,1:3) * currSURF3D + repmat(extCam2World(1:3,4),1,size(currSURF3D,2)); - currSURFpts3D = [currSURFpts3D currSURF3D]; - end - - % Only use SURF points with valid depth - validDepth = find(currSURFpts3D(3,:) > 0.15); - currFrameSURF = currFrameSURF(validDepth); - currFrameDesc = currFrameDesc(validDepth,:); - currSURFpts3D = currSURFpts3D(:,validDepth); - allSURF2D{frameIdx} = currFrameSURF; - allSURFDesc{frameIdx} = currFrameDesc; - allSURF3D{frameIdx} = currSURFpts3D; - - % Get index of previous frame - if frameIdx == 1 - allExtCam2World{frameIdx} = extCam2World; - dlmwrite(fullfile(savetoPath,extCam2WorldFiles(frameIdx).name),allExtCam2World{frameIdx},'delimiter',' '); - continue; - elseif mod(frameIdx,3) == 1 - prevFrameIdx = frameIdx - 3; - else - prevFrameIdx = frameIdx - 1; - end - - % Get SURF features of previous frame - prevFrameSURF = allSURF2D{prevFrameIdx}; - prevFrameDesc = allSURFDesc{prevFrameIdx}; - prevSURFpts3D = allSURF3D{prevFrameIdx}; - - % Match SURF features - matchInd = matchFeatures(prevFrameDesc,currFrameDesc); - prevMatchSURF = prevFrameSURF(matchInd(:,1)); - currMatchSURF = currFrameSURF(matchInd(:,2)); - prevMatchPts3D = prevSURFpts3D(:,matchInd(:,1)); - currMatchPts3D = currSURFpts3D(:,matchInd(:,2)); -% showMatchedFeatures(allColorIm{prevFrameIdx},allColorIm{frameIdx},prevMatchSURF,currMatchSURF); - - [initRt, ransacInliers] = ransacfitRt([prevMatchPts3D;currMatchPts3D], 0.005,0); - fprintf('RANSAC inliers: %d/%d\n',size(ransacInliers,2),size(currMatchPts3D,2)); - - allExtCam2World{frameIdx} = allExtCam2World{prevFrameIdx} * [initRt; [0 0 0 1]]; - currExtCam2World = allExtCam2World{frameIdx}; -% currCamPts = currExtCam2World(1:3,1:3) * allCamPts{frameIdx} + repmat(currExtCam2World(1:3,4),1,size(allCamPts{frameIdx},2)); -% currCloud = pointCloud(currCamPts','Color',allColorPts{frameIdx}'); -% pcwrite(currCloud,sprintf('%d',frameIdx),'PLYFormat','binary'); - dlmwrite(fullfile(savetoPath,extCam2WorldFiles(frameIdx).name),currExtCam2World,'delimiter',' '); - -end - - - - - - - - - - - - - - diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/demo.m b/ros-packages/catkin_ws/src/pose_estimation/src/demo.m new file mode 100644 index 0000000..4645fd3 --- /dev/null +++ b/ros-packages/catkin_ws/src/pose_estimation/src/demo.m @@ -0,0 +1,131 @@ +% User configurations (change me) +toolboxPath = '/home/andyz/apc/toolbox'; % Directory of toolbox utilities +tmpDataPath = '/home/andyz/apc/toolbox/data/tmp'; % Temporary directory used by marvin_convnet, where all RGB-D images and detection masks are saved +modelsPath = './models/objects'; % Directory holding pre-scanned object models +scenePath = fullfile(toolboxPath,'data/sample/scene-0000'); % Directory holding the RGB-D data of the sample scene +calibPath = fullfile(toolboxPath,'data/sample/calibration'); % Directory holding camera pose calibration data for the sample scene + +% Add paths and create directories +addpath(genpath(fullfile(toolboxPath,'rgbd-utils'))); +addpath(genpath(fullfile(toolboxPath,'vis-utils'))); + +% Get set of colors for visualizations (from vis-utils) +load('colorPalette.mat'); + +% Remove all files in temporary folder used by marvin_convnet +if exist(fullfile(tmpDataPath,'raw'),'file') + rmdir(fullfile(tmpDataPath,'raw'),'s'); +end +if exist(fullfile(tmpDataPath,'results'),'file') + rmdir(fullfile(tmpDataPath,'results'),'s'); +end +if exist(fullfile(tmpDataPath,'HHA'),'file') + rmdir(fullfile(tmpDataPath,'HHA'),'s'); +end +if exist(fullfile(tmpDataPath,'masks'),'file') + rmdir(fullfile(tmpDataPath,'masks'),'s'); +end +if exist(fullfile(tmpDataPath,'segm'),'file') + rmdir(fullfile(tmpDataPath,'segm'),'s'); +end +delete(fullfile(tmpDataPath,'*')); + +% Copy current scene to temporary folder used by marvin_convnet +copyfile(fullfile(scenePath,'*'),tmpDataPath); + +% Load sample scene data +fprintf(' [Processing] Loading scene RGB-D data\n'); +sceneData = loadScene(tmpDataPath); +numFrames = length(sceneData.colorFrames); + +% Calibrate scene +sceneData = loadCalib(calibPath,sceneData); + +% Fill holes in depth frames for scene and make HHA +if ~exist(fullfile(tmpDataPath,'HHA'),'file') + fprintf(' [Processing] Creating HHA maps from depth data\n'); + mkdir(fullfile(tmpDataPath,'HHA')); + for frameIdx = 1:length(sceneData.depthFrames) + sceneData.depthFrames{frameIdx} = fillHoles(sceneData.depthFrames{frameIdx}); + HHA = getHHA(sceneData.env,sceneData.depthFrames{frameIdx},sceneData.colorK,sceneData.extCam2World{frameIdx},sceneData.extBin2World); + imwrite(HHA,fullfile(tmpDataPath,'HHA',sprintf('frame-%06d.HHA.png',frameIdx-1))); + end +end + +% Call marvin_convnet to do 2D object segmentation for each RGB-D frame +fprintf(' [Segmentation] Frame '); +if strcmp(sceneData.env,'shelf') + binIds = 'ABCDEFGHIJKL'; + binNum = strfind(binIds,sceneData.binId)-1; +else + binNum = -1; +end +for frameIdx = 0:(numFrames-1) + fprintf('%d ',frameIdx); + [client,reqMsg] = rossvcclient('/marvin_convnet'); + reqMsg.BinId = binNum; + reqMsg.ObjectNames = sceneData.objects; + reqMsg.FrameId = frameIdx; + response = call(client,reqMsg); +end +fprintf('\n'); + +% Call pose_estimation to do 6D object pose estimation for the sequence +[client,reqMsg] = rossvcclient('/pose_estimation'); +reqMsg.SceneFiles = tmpDataPath; +reqMsg.CalibrationFiles = calibPath; +reqMsg.DoCalibration = true; +try + respMsg= call(client,reqMsg); +catch +end + +fprintf(' [Visualization] Drawing predicted object poses\n'); +% Load results (predicted 6D object poses) +resultFiles = dir(fullfile(tmpDataPath,'results/*.result.txt')); +results = cell(1,length(resultFiles)); +confScores = []; +for resultIdx = 1:length(resultFiles) + tmpResultFile = resultFiles(resultIdx).name; + tmpResultFilenameDotIdx = strfind(tmpResultFile,'.'); + tmpResult.objName = tmpResultFile(1:(tmpResultFilenameDotIdx(1)-1)); + tmpResult.objNum = str2double(tmpResultFile((tmpResultFilenameDotIdx(1)+1):(tmpResultFilenameDotIdx(2)-1))); + tmpResult.objPoseWorld = eye(4); + tmpResult.objPoseWorld(1:3,4) = dlmread(fullfile(tmpDataPath,'results',tmpResultFile),'\t',[1,0,1,2])'; + objPoseRotQuat = dlmread(fullfile(tmpDataPath,'results',tmpResultFile),'\t',[4,0,4,3]); + tmpResult.objPoseWorld(1:3,1:3) = quat2rotm([objPoseRotQuat(4),objPoseRotQuat(1:3)]); + tmpResult.confScore = dlmread(fullfile(tmpDataPath,'results',tmpResultFile),'\t',[28,0,28,0]); + confScores = [confScores;tmpResult.confScore]; + results{resultIdx} = tmpResult; +end + +% Sort results by confidence scores +[~,sortIdx] = sortrows(confScores,1); + +% Get random colors per object in the scene +randColorIdx = randperm(length(colorPalette),length(results)); + +% Create canvas for visualization +canvas = sceneData.colorFrames{8}; +canvasSeg = uint8(ones(size(canvas))*255); +canvasPose = canvas; +canvasPose = insertText(canvasPose,[10 10],'Confidence : Object','Font','LucidaSansDemiBold','FontSize',12,'TextColor','white','BoxColor','black'); + +% Loop through each object to save and visualize predicted object pose +uniqueObjList = unique(sceneData.objects); +textPosY = 32; +for resultIdx = 1:length(resultFiles) + currResult = results{sortIdx(resultIdx)}; + objColor = colorPalette{randColorIdx(resultIdx)}; + + % Load pre-scanned object point cloud + objPointCloud = pcread(fullfile(modelsPath,sprintf('%s.ply',currResult.objName))); + + % Draw projected object model and bounding box + [canvasSeg,canvasPose] = showObjectPose(currResult.objName, canvasSeg, canvasPose, sceneData.colorFrames{8}, sceneData.depthFrames{8}, sceneData.extCam2World{8}, sceneData.colorK, objPointCloud, currResult.objPoseWorld, objColor); + canvasPose = insertText(canvasPose,[10 textPosY],sprintf(' %f : %s',currResult.confScore,currResult.objName),'Font','LucidaSansDemiBold','FontSize',12,'TextColor',objColor,'BoxColor','black'); + textPosY = textPosY + 22; +end + +imshow(canvasPose); title('Predicted 6D Object Poses'); + diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/denoisePointCloud.m b/ros-packages/catkin_ws/src/pose_estimation/src/denoisePointCloud.m index 9f2a1fb..d2cff52 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/denoisePointCloud.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/denoisePointCloud.m @@ -1,8 +1,25 @@ -function objSegPts = denoisePointCloud(objSegPts) +function objSegmPts = denoisePointCloud(objSegmPts) +% Remove outlier points from the principal components (computed from PCA) +% of a noisy point cloud +% +% function objSegPts = denoisePointCloud(objSegPts) +% Input: +% objSegPts - 3xN float array of 3D points +% Output: +% objSegPts - 3xN float array of 3D points +% +% --------------------------------------------------------- +% Copyright (c) 2016, Andy Zeng +% +% This file is part of the APC Vision Toolbox and is available +% under the terms of the Simplified BSD License provided in +% LICENSE. Please retain this notice and LICENSE if you use +% this file (or any portion of it) in your project. +% --------------------------------------------------------- % Compute PCA for removing outliers from segmented point cloud -coeffPCAoutlier = pca(objSegPts'); -currObjSegPtsAligned = (inv(coeffPCAoutlier) * (objSegPts - repmat(median(objSegPts,2),1,size(objSegPts,2))))'; +coeffPCAoutlier = pca(objSegmPts'); +currObjSegPtsAligned = (inv(coeffPCAoutlier) * (objSegmPts - repmat(median(objSegmPts,2),1,size(objSegmPts,2))))'; % Find outliers from first principal component (local minima < 5%, maxima > 50%) currObjSegPtsDistPC1 = currObjSegPtsAligned(:,1); @@ -50,7 +67,7 @@ PC3Range = (min(sortedObjSegPtsDistPC3)-0.005 + (PC3Range-1)*0.01); % Remove outliers -objSegPts = objSegPts(:,find((currObjSegPtsDistPC1 > PC1Range(1)) & (currObjSegPtsDistPC1 < PC1Range(2)) & ... +objSegmPts = objSegmPts(:,find((currObjSegPtsDistPC1 > PC1Range(1)) & (currObjSegPtsDistPC1 < PC1Range(2)) & ... (currObjSegPtsDistPC2 > PC2Range(1)) & (currObjSegPtsDistPC2 < PC2Range(2)) & ... (currObjSegPtsDistPC3 > PC3Range(1)) & (currObjSegPtsDistPC3 < PC3Range(2)))); diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getCameraPointClouds.m b/ros-packages/catkin_ws/src/pose_estimation/src/getCameraPointClouds.m deleted file mode 100644 index 454f149..0000000 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getCameraPointClouds.m +++ /dev/null @@ -1,89 +0,0 @@ -function [data,bgCloud,camCloud,bin2BgRt] = getCameraPointClouds(dataPath,totePosePath,shelfPosePath,visPath,binID,savePointClouds) - -% Rviz parameters -rvizPC = false; global vizpub; - -% Parameters specific to shelf or tote scenario -if binID == -1 - frames = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18]; - camPosePath = totePosePath; -else - frames = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15]; - camPosePath = shelfPosePath; -end - -% Get RGB-D frame data and camera poses -data = getData(dataPath,camPosePath,frames,binID); - -% Create aggregated RGB-D point cloud (15 frames) -for frameIdx = frames - color = data.color{frameIdx}; - depth = data.depth{frameIdx}; - extCam2Bin = data.extWorld2Bin * data.extCam2World{frameIdx}; - - % Project depth into camera space - [pixX,pixY] = meshgrid(1:640,1:480); - camX = (pixX-data.K(1,3)).*depth/data.K(1,1); - camY = (pixY-data.K(2,3)).*depth/data.K(2,2); - camZ = depth; - - % Only use points with valid depth - validDepth = find((camZ > 0) & (camZ < 1)); - camPts = [camX(validDepth),camY(validDepth),camZ(validDepth)]'; - camPts = extCam2Bin(1:3,1:3) * camPts + repmat(extCam2Bin(1:3,4),1,size(camPts,2)); - - % Get vertex colors - colorR = color(:,:,1); - colorG = color(:,:,2); - colorB = color(:,:,3); - colorPts = [colorR(validDepth),colorG(validDepth),colorB(validDepth)]'; - - % Aggregate point clouds - if frameIdx == frames(1) - camCloud = pointCloud(camPts','Color',colorPts'); - else - camCloud = pcmerge(camCloud,pointCloud(camPts','Color',colorPts'),0.002); - end -end - -if savePointClouds - pcwrite(camCloud,fullfile(visPath,'vis.camCloud'),'PLYFormat','binary'); -end -if rvizPC - send(vizpub, createPointCloudMarker(sprintf('vis.camCloud'), allCamPts, [1 1 1], allColorPts, sprintf('bin%d', binID), 100)); -end - -% Save denoised and downsampled point cloud for planning -% if savePointClouds -% sparseCamCloud = pcdenoise(camCloud,'NumNeighbors',4*15); -% sparseCamCloud = pcdownsample(sparseCamCloud,'gridAverage',0.0025); -% if binID == -1 -% viewBounds = [-0.3, 0.3; -0.4, 0.4; -0.05, 0.2]; -% else -% viewBounds = [-0.10, 0.40; -0.17, 0.17; -0.06, 0.20]; -% end -% sparseCamPts = sparseCamCloud.Location'; -% sparseCamColors = sparseCamCloud.Color'; -% ptsOutsideBounds = find((sparseCamPts(1,:) < viewBounds(1,1)) | (sparseCamPts(1,:) > viewBounds(1,2)) | ... -% (sparseCamPts(2,:) < viewBounds(2,1)) | (sparseCamPts(2,:) > viewBounds(2,2)) | ... -% (sparseCamPts(3,:) < viewBounds(3,1)) | (sparseCamPts(3,:) > viewBounds(3,2))); -% sparseCamPts(:,ptsOutsideBounds) = []; -% sparseCamColors(:,ptsOutsideBounds) = []; -% sparseCamCloud = pointCloud(sparseCamPts','Color',sparseCamColors'); -% pcwrite(sparseCamCloud,fullfile(visPath,'vis.sparse'),'PLYFormat','binary'); -% end - -% plot3(allCamPts(1,:)',allCamPts(2,:)',allCamPts(3,:)','.r'); -% hold on; axis equal; grid on; xlabel('x'); ylabel('y'); zlabel('z'); hold off; - -% Align empty bin to observation -bgCloud = pcread(sprintf('bins/bin%d.ply',binID)); -[tform,bgCloud] = pcregrigidGPU(bgCloud,camCloud,'InlierRatio',0.8,'MaxIterations',200,'Tolerance',[0.0001 0.0009],'Verbose',false,'Extrapolate',true); -bin2BgRt = inv(tform.T'); - -if savePointClouds - pcwrite(bgCloud,fullfile(visPath,'vis.bgCloud'),'PLYFormat','binary'); -end - -end - diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getData.m b/ros-packages/catkin_ws/src/pose_estimation/src/getData.m deleted file mode 100644 index a41d6af..0000000 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getData.m +++ /dev/null @@ -1,33 +0,0 @@ -function data = getData(dataPath,camPosePath,frames,binID) - -% Search through files in data directory -colorFiles = dir(fullfile(dataPath,'*.color.png')); -depthFiles = dir(fullfile(dataPath,'*.regdepth.png')); -if binID == -1 - extFiles = dir(fullfile(camPosePath,'*.pose_camera_map.txt')); -else - extFiles = dir(fullfile(dataPath,'*.pose_camera_map.txt')); -end - -% Load camera intrinsics -data.K = dlmread(fullfile(dataPath,'color_intrinsics.K.txt')); - -% Load RGB-D images and camera poses -data.extBin2World = dlmread(fullfile(dataPath,sprintf('pose_bin%d_map.txt',binID))); -data.extWorld2Bin = inv(data.extBin2World); -data.color = {}; -data.depth = {}; -data.extCam2World = {}; -for frameIdx = frames - data.color{frameIdx} = imread(fullfile(dataPath,colorFiles(frameIdx).name)); - data.depth{frameIdx} = double(imread(fullfile(dataPath,depthFiles(frameIdx).name)))./10000; - if binID == -1 - data.extCam2World{frameIdx} = dlmread(fullfile(camPosePath,extFiles(frameIdx).name)); - else - midExtCam2World = dlmread(fullfile(dataPath,extFiles(8).name)); - data.extCam2World{frameIdx} = midExtCam2World * dlmread(fullfile(camPosePath,sprintf('%06d',binID),extFiles(frameIdx).name)); - end -end - -end - diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getEmptyObjectHypothesis.m b/ros-packages/catkin_ws/src/pose_estimation/src/getEmptyObjectHypothesis.m index eacff4a..dc9434b 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getEmptyObjectHypothesis.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/getEmptyObjectHypothesis.m @@ -1,4 +1,14 @@ function objectHypothesis = getEmptyObjectHypothesis(scenePath,objName,instanceIdx) +% Return ROS message with empty object pose +% +% --------------------------------------------------------- +% Copyright (c) 2016, Andy Zeng +% +% This file is part of the APC Vision Toolbox and is available +% under the terms of the Simplified BSD License provided in +% LICENSE. Please retain this notice and LICENSE if you use +% this file (or any portion of it) in your project. +% --------------------------------------------------------- % Save object pose to ROS message poseTrans = rosmessage('geometry_msgs/Point'); @@ -55,8 +65,20 @@ objectHypothesis.Score = 0; % Save ROS message to file -fid = fopen(fullfile(scenePath,strcat(objName,sprintf('.%d.result.txt',instanceIdx))),'w'); -fprintf(fid,'%f\n',zeros(27,1)); +if ~exist(fullfile(scenePath,'results'),'file') + mkdir(fullfile(scenePath,'results')); +end +fid = fopen(fullfile(scenePath,'results',strcat(objName,sprintf('.%d.result.txt',instanceIdx))),'w'); +fprintf(fid,'# Predicted object pose translation (x,y,z in world coordinates)\n%15.8e\t %15.8e\t %15.8e\t\n\n',0,0,0); +fprintf(fid,'# Predicted object pose rotation (x,y,z,w object-to-world quaternion)\n%15.8e\t %15.8e\t %15.8e\t %15.8e\t\n\n',0,0,0,0); +fprintf(fid,'# Segmented object point cloud median (x,y,z in world coordinates)\n%15.8e\t %15.8e\t %15.8e\t\n\n',0,0,0); +fprintf(fid,'# Segmented object point cloud PCA rotation (x,y,z,w object-to-world quaternion)\n%15.8e\t %15.8e\t %15.8e\t %15.8e\t\n\n',0,0,0,0); +fprintf(fid,'# Segmented object point cloud PCA variance (in PC directions)\n%15.8e\t %15.8e\t %15.8e\t\n\n',0,0,0); +fprintf(fid,'# Segmented object point cloud centroid/mean (x,y,z in world coordinates)\n%15.8e\t %15.8e\t %15.8e\t\n\n',0,0,0); +fprintf(fid,'# Segmented object point cloud x-range in world coordinates (bounding box in x-direction)\n%15.8e\t %15.8e\t\n\n',0,0); +fprintf(fid,'# Segmented object point cloud y-range in world coordinates (bounding box in y-direction)\n%15.8e\t %15.8e\t\n\n',0,0); +fprintf(fid,'# Segmented object point cloud z-range in world coordinates (bounding box in z-direction)\n%15.8e\t %15.8e\t\n\n',0,0); +fprintf(fid,'# Prediction confidence score\n%.17g\n',0); fclose(fid); end diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m index 8a1339e..f56c112 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m @@ -1,4 +1,14 @@ function objectHypothesis = getObjectHypothesis(surfPCAPoseWorld,latentPCA,surfCentroid,surfRangeWorld,predObjPoseWorld,predObjConfScore,scenePath,objName,instanceIdx) +% Return ROS message with predicted object pose information +% +% --------------------------------------------------------- +% Copyright (c) 2016, Andy Zeng +% +% This file is part of the APC Vision Toolbox and is available +% under the terms of the Simplified BSD License provided in +% LICENSE. Please retain this notice and LICENSE if you use +% this file (or any portion of it) in your project. +% --------------------------------------------------------- % Save object pose to ROS message poseTrans = rosmessage('geometry_msgs/Point'); diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectMasks.m b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectMasks.m index 5f4899e..4a00135 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectMasks.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectMasks.m @@ -1,14 +1,39 @@ -function [objMasks,segmThresh,segmConfMaps] = getObjectMasks(dataPath,objName,frames) +function [objMasks,segmThresh,segmConfMaps] = getObjectMasks(scenePath,objName,frames) +% Take in segmentation confidence maps, compute a confidence threshold, and +% return binary masks +% +% function [objMasks,segmThresh,segmConfMaps] = getObjectMasks(dataPath,objName,frames) +% Input: +% scenePath - file path to the folder holding RGB-D data of the scene, +% which also has segmentation results from marvin_convnet +% objName - name of the target object (aka. object ID) +% frames - 1xN array indicating which frames of the RGB-D sequence +% from the scene to use +% Output: +% objMasks - 1xN cell array of 480x640 binary object masks (computed +% from segmentation) +% segmThresh - segmentation confidence threshold +% segmConfMaps - 1xN cell array of 480x640 confidence values from +% segmentation +% +% --------------------------------------------------------- +% Copyright (c) 2016, Andy Zeng +% +% This file is part of the APC Vision Toolbox and is available +% under the terms of the Simplified BSD License provided in +% LICENSE. Please retain this notice and LICENSE if you use +% this file (or any portion of it) in your project. +% --------------------------------------------------------- % Search through mask files in data directory -maskFiles = dir(fullfile(dataPath,'masks',sprintf('*.%s.mask.png',objName))); +maskFiles = dir(fullfile(scenePath,'masks',sprintf('*.%s.mask.png',objName))); % Load segmentation confidence maps objMasks = {}; segmConfMaps = {}; segSum = zeros(480,640); for frameIdx = frames - segConf = double(imread(fullfile(dataPath,'masks',maskFiles(frameIdx).name)))./65535; + segConf = double(imread(fullfile(scenePath,'masks',maskFiles(frameIdx).name)))./65535; segSum = segSum + segConf; segmConfMaps{frameIdx} = segConf; end diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m index 128b2f7..4934cf6 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m @@ -1,4 +1,36 @@ function objHypotheses = getObjectPose(scenePath,sceneData,scenePointCloud,backgroundPointCloud,extBin2Bg,objName,objModel,objNum) +% Take in scene RGB-D data and information about a target object, and +% predict the 6D pose for that object. +% +% function objHypotheses = getObjectPose(scenePath,sceneData,scenePointCloud,backgroundPointCloud,extBin2Bg,objName,objModel,objNum) +% Input: +% scenePath - file path to the folder holding RGB-D data of +% the scene +% sceneData - data structure holding the contents (frames and +% camera information) of a captured scene (has N +% RGB-D frames) +% scenePointCloud - data structure holding the point cloud of the scene +% backgroundPointCloud - data structure holding the background point cloud +% extBin2Bg - 4x4 rigid transformation matrix (in homogeneous +% coordinates) aligning the background point cloud +% to the scene point cloud +% objName - name of the target object (aka. object ID) +% objModel - data structure holding the point cloud of the +% pre-scanned object model +% objNum - number of instances of the target object in the +% scene +% Output: +% objHypotheses - ROS message with predicted 6D object pose (and +% other information about the surface point cloud) +% +% --------------------------------------------------------- +% Copyright (c) 2016, Andy Zeng +% +% This file is part of the APC Vision Toolbox and is available +% under the terms of the Simplified BSD License provided in +% LICENSE. Please retain this notice and LICENSE if you use +% this file (or any portion of it) in your project. +% --------------------------------------------------------- global visPath; global savePointCloudVis; @@ -14,20 +46,35 @@ % Parameters specific to shelf or tote scenario if strcmp(sceneData.env,'tote') frames = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18]; +% frames = [1 3 5 7 9 10 12 14 16 18]; +% frames = [5 14]; viewBounds = [-0.3, 0.3; -0.4, 0.4; -0.05, 0.2]; pushBackAxis = [0; 0; -1]; else frames = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15]; +% frames = [1 5 8 11 15]; +% frames = 8; viewBounds = [-0.01, 0.40; -0.17, 0.17; -0.06, 0.20]; pushBackAxis = [1; 0; -1]; end -% Parse segmentation masks and return confidence threshold used +% Parse segmentation masks and save confidence threshold used [objMasks,segmThresh,segmConfMaps] = getObjectMasks(scenePath,objName,frames); +dlmwrite(fullfile(scenePath,'masks',sprintf('%s.thresh.txt',objName)),segmThresh); % Create segmented point cloud of object [objSegmPts,objSegmConf] = getSegmentedPointCloud(sceneData,frames,objMasks,segmConfMaps); +% If no segmentation, return dummy pose +if size(objSegmPts,2) < 200 + fprintf(' [Pose Estimation] %s: 0.000000\n',objName); + for instanceIdx = 1:objNum + currObjHypothesis = getEmptyObjectHypothesis(scenePath,objName,instanceIdx); + objHypotheses = [objHypotheses,currObjHypothesis]; + end + return; +end + % Handle objects without depth if strcmp(objName,'dasani_water_bottle') && strcmp(sceneData.env,'shelf') || ... strcmp(objName,'rolodex_jumbo_pencil_cup') || ... @@ -132,13 +179,13 @@ % Print segmentation confidence predObjConfScore = mean(instanceConf{instanceIdx}); - if strcmp(objName,'dasani_water_bottle') && binID == -1 - predObjConfScore = predObjConfScore/4; + if strcmp(objName,'dasani_water_bottle') && strcmp(sceneData.env,'tote') +% predObjConfScore = predObjConfScore/4; end if size(currObjSegmPts,2) < 100 % If object not found fprintf(' [Pose Estimation] %s: 0.000000\n',objName); - currObjHypothesis = getEmptyObjectHypothesis(dataPath,objName,instanceIdx); - objHypotheses = [objHypotheses currObjHypothesis]; + currObjHypothesis = getEmptyObjectHypothesis(scenePath,objName,instanceIdx); + objHypotheses = [objHypotheses,currObjHypothesis]; continue; else fprintf(' [Pose Estimation] %s: %f\n',objName,predObjConfScore); @@ -281,18 +328,6 @@ end % Save pose as a rigid transform from model to world space - -% tmpAxisPts = finalRt(1:3,1:3) * axisPts + repmat(finalRt(1:3,4),1,size(axisPts,2)); -% pcwrite(pointCloud(tmpAxisPts','Color',axisColors'),fullfile(visPath,sprintf('vis.objAxis.%s.%d',objName,instanceIdx)),'PLYFormat','binary'); - -% if (finalRt(1) < viewBounds(1,1)) || (finalRt(1) > viewBounds(1,2)) || ... -% (finalRt(2) < viewBounds(2,1)) || (finalRt(2) > viewBounds(2,2)) || ... -% (finalRt(3) < viewBounds(3,1)) || (finalRt(3) > viewBounds(3,2)) -% currObjectHypothesis = getEmptyObjectHypothesis(dataPath,objName,instanceIdx); -% objectHypotheses = [objectHypotheses currObjectHypothesis]; -% continue; -% end - predObjPoseWorld = sceneData.extBin2World * predObjPoseBin; if saveResultImageVis visualizeResults(surfPCAPoseWorld,latentPCA,surfCentroid,surfRangeWorld,predObjPoseWorld,predObjConfScore,scenePath,objName,instanceIdx,sceneData,fullCurrObjSegmPts,objMasks,objModel.Location'); diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPoseNoDepth.m b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPoseNoDepth.m index b4a00ea..27d5fba 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPoseNoDepth.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPoseNoDepth.m @@ -1,4 +1,34 @@ function [predObjPoseBin,surfCentroid,surfRange] = getObjectPoseNoDepth(visPath,objSegmPts,objName,frames,sceneData,objMasks) +% Take in scene RGB-D data and segmentation confidence maps of a particular +% object (with no depth) and predict a 6D pose using triangulation +% +% function [predObjPoseBin,surfCentroid,surfRange] = getObjectPoseNoDepth(visPath,objSegmPts,objName,frames,sceneData,objMasks) +% Input: +% visPath - directory to save point cloud visualization files +% objSegmPts - 3xK float array of K 3D points of the segmented object +% objName - name of the target object (aka. object ID) +% frames - 1xN array indicating which frames of the RGB-D sequence +% from sceneData to use +% sceneData - data structure holding the contents (frames and camera +% information) of a captured scene with N RGB-D frames +% objMasks - 1xN cell array of 480x640 binary object masks (computed +% from segmentation) +% segConfMaps - 1xN cell array of 480x640 confidence values from +% segmentation +% Output: +% predObjPoseBin - 4x4 predicted object pose in bin's coordinate system +% object-to-bin (homogenous coordinates) +% surfCentroid - predicted centroid of the object +% surfRange - predicted object 3D bounding box +% +% --------------------------------------------------------- +% Copyright (c) 2016, Andy Zeng +% +% This file is part of the APC Vision Toolbox and is available +% under the terms of the Simplified BSD License provided in +% LICENSE. Please retain this notice and LICENSE if you use +% this file (or any portion of it) in your project. +% --------------------------------------------------------- global savePointCloudVis; @@ -23,22 +53,10 @@ binGridPtsPix(1,:) = round((binGridPtsPix(1,:).*sceneData.colorK(1,1))./binGridPtsPix(3,:)+sceneData.colorK(1,3)); binGridPtsPix(2,:) = round((binGridPtsPix(2,:).*sceneData.colorK(2,2))./binGridPtsPix(3,:)+sceneData.colorK(2,3)); pixWithinImage = find((binGridPtsPix(1,:) <= 640) & (binGridPtsPix(1,:) > 0) & (binGridPtsPix(2,:) <= 480) & (binGridPtsPix(2,:) > 0)); -% binGridPtsPix = binGridPtsPix(:,pixWithinImage); -% binGridPts = binGridPts(:,pixWithinImage); occupiedBinGridInd = pixWithinImage(find(currObjMask(sub2ind(size(currObjMask),binGridPtsPix(2,pixWithinImage)',binGridPtsPix(1,pixWithinImage)')) > 0)); -% currBinGridPts = binGridPts(:,occupiedBinGridInd); -% imshow(objMasks{frameIdx}) -% hold on; plot(binGridPtsPix(1,:)',binGridPtsPix(2,:)','.b'); hold off; -% binGridPtsPix = binGridPtsPix(:,occupiedBinGridInd); binGridOccupancy(occupiedBinGridInd) = binGridOccupancy(occupiedBinGridInd) + 1; -% hold on; plot(binGridPtsPix(1,:)',binGridPtsPix(2,:)','.r'); hold off; end -% objectPoseNoDepth = getObjectPoseNoDepth(objName,) -% getObjectHypothesis(RtPCA,latentPCA,finalRt,finalScore,dataPath,objName,instanceIdx); - - - predObjPoseBin = eye(4); bestGuessPtsInd = find(binGridOccupancy > gridOccupancyThreshold); @@ -49,7 +67,9 @@ instanceIdx = 1; objPts = sortrows(objPts',1); objCentroid = median(objPts); -predObjPoseBin(1:3,4) = objCentroid'; +if ~isempty(objPts) + predObjPoseBin(1:3,4) = objCentroid'; +end if strcmp(sceneData.env,'shelf') if strcmp(objName,'dasani_water_bottle') diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getSegmentedPointCloud.m b/ros-packages/catkin_ws/src/pose_estimation/src/getSegmentedPointCloud.m index c8982b9..491775a 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getSegmentedPointCloud.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/getSegmentedPointCloud.m @@ -1,4 +1,30 @@ function [objSegmPts,objSegmConf] = getSegmentedPointCloud(sceneData,frames,objMasks,segConfMaps) +% Take in scene RGB-D data and segmentation confidence maps of a particular +% object to return a 3D point cloud of the segmented object +% +% function objSegPts = denoisePointCloud(objSegPts) +% Input: +% sceneData - data structure holding the contents (frames and camera +% information) of a captured scene (has N RGB-D frames) +% frames - 1xN array indicating which frames of the RGB-D sequence +% from sceneData to use +% objMasks - 1xN cell array of 480x640 binary object masks (computed +% from segmentation) +% segConfMaps - 1xN cell array of 480x640 confidence values from +% segmentation +% Output: +% objSegmPts - 3xK float array of K 3D points of the segmented object +% objSegmConf - 1xK float array of confidence values for each of the K +% points of the segmented object +% +% --------------------------------------------------------- +% Copyright (c) 2016, Andy Zeng +% +% This file is part of the APC Vision Toolbox and is available +% under the terms of the Simplified BSD License provided in +% LICENSE. Please retain this notice and LICENSE if you use +% this file (or any portion of it) in your project. +% --------------------------------------------------------- % Create segmented object point cloud objSegmPts = []; diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/makeShelfBin.m b/ros-packages/catkin_ws/src/pose_estimation/src/makeShelfBin.m deleted file mode 100644 index d73c803..0000000 --- a/ros-packages/catkin_ws/src/pose_estimation/src/makeShelfBin.m +++ /dev/null @@ -1,96 +0,0 @@ - -binID = 11; -gridSampleSize = 0.002; - -% datapath = sprintf('/home/mcube/apcdata/princeton_data/raw/tote_test3/empty'); -% totePosePath = '/home/mcube/apcdata/princeton_data/raw/tote_align/poses'; -% datapath = sprintf('/home/mcube/apcdata/princeton_data/raw/tote_align/empty'); -% totePosePath = '/home/mcube/apcdata/princeton_data/raw/tote_align/poses'; -datapath = sprintf('/home/mcube/apcdata/princeton_data/raw/shelf_align/empty/000011'); -totePosePath = '/home/mcube/apcdata/princeton_data/raw/shelf_align/poses/000011'; - -if binID ~= -1 - posePath = datapath; -else - posePath = totePosePath; -end - - -% Read RGB-D frames -fprintf('Reading RGB-D frames.\n'); -depthFiles = dir(fullfile(datapath,'*.regdepth.png')); -colorFiles = dir(fullfile(datapath,'*.color.png')); -extFiles = dir(fullfile(posePath,'*.pose_camera_map.txt')); -extWorld2Bin = inv(dlmread(fullfile(datapath,sprintf('pose_bin%d_map.txt',binID)))); -depthArray = {}; -colorImArray = {}; -extCam2WorldArray = {}; -for frameIdx = 1:length(depthFiles) - depthArray{frameIdx} = double(imread(fullfile(datapath,depthFiles(frameIdx).name)))./10000; - colorImArray{frameIdx} = imread(fullfile(datapath,colorFiles(frameIdx).name)); - extCam2WorldArray{frameIdx} = dlmread(fullfile(posePath,extFiles(frameIdx).name)); -end - -% Get camera info -colorK = dlmread(fullfile(datapath,'color_intrinsics.K.txt')); -% depthK = dlmread(fullfile(datapath,'depth_intrinsics.K.txt')); -% depth2colorExt = dlmread(fullfile(datapath,'depth2color_extrinsics.K.txt')); - -% Create aggregated RGB-D point cloud (15 frames) -fprintf('Aggregating RGB-D point cloud.\n'); -allCamPts = []; -allColorPts = []; -objCamPts = []; -for frameIdx = 1:length(depthFiles) - depth = depthArray{frameIdx}; - colorIm = colorImArray{frameIdx}; - extCam2World = extCam2WorldArray{frameIdx}; - extCam2Bin = extWorld2Bin * extCam2World; - - % Project depth into camera space - [pixX,pixY] = meshgrid(1:640,1:480); - camX = (pixX-colorK(1,3)).*depth/colorK(1,1); - camY = (pixY-colorK(2,3)).*depth/colorK(2,2); - camZ = depth; - - % Only use points with valid depth - validDepth = intersect(find(camZ > 0),find(camZ < 1)); - camPts = [camX(validDepth),camY(validDepth),camZ(validDepth)]'; - camPts = extCam2Bin(1:3,1:3) * camPts + repmat(extCam2Bin(1:3,4),1,size(camPts,2)); - allCamPts = [allCamPts camPts]; - - % Get vertex colors - colorR = colorIm(:,:,1); - colorG = colorIm(:,:,2); - colorB = colorIm(:,:,3); - colorPts = [colorR(validDepth),colorG(validDepth),colorB(validDepth)]'; - allColorPts = [allColorPts colorPts]; - -% pcwrite(pointCloud(camPts','Color',colorPts'),fullfile('bins',sprintf('test%d',frameIdx)),'PLYFormat','binary'); -end - -ptCloud = pointCloud(allCamPts','Color',allColorPts'); -ptCloud = pcdownsample(ptCloud,'gridAverage',0.005); -pcwrite(ptCloud,fullfile('bins',sprintf('bin%d',binID)),'PLYFormat','binary'); - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/centerObject.m b/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/centerObject.m deleted file mode 100644 index 477c6be..0000000 --- a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/centerObject.m +++ /dev/null @@ -1,69 +0,0 @@ -clear all; close all; - -% Load object -% objCloud = offLoader('data/models/book_joke.off'); -objName = 'dasani_water_bottle'; -objCloud = pcread(sprintf('%s.ply',objName)); - -% % Downsample object -% objCloud = pcdownsample(objCloud,'gridAverage',0.001); -% objCloud = pcdenoise(objCloud,'NumNeighbors',4); - -% Rotate with respect to PCA, major axis = x -% coeff = pca(objCloud.vmat'); -% objCloud.vmat = inv(coeff) * objCloud.vmat; -objCloudDownsampled = pcdownsample(objCloud,'gridAverage',0.005); -objPts = objCloud.Location'; -coeff = pca(objCloudDownsampled.Location); -objPts = inv(coeff) * objPts; - -% % Make z-up for now -% objCloud.vmat = [0 0 1;0 1 0;1 0 0] * objCloud.vmat; - -% Center object -objLimits = [min(objPts,[],2),max(objPts,[],2)]; -objCenter = mean(objLimits,2); -objPts = objPts - repmat(objCenter,1,size(objPts,2)); - -% % Fix normals -% newFmat = objCloud.fmat; -% newFmat(2,:) = objCloud.fmat(3,:); -% newFmat(3,:) = objCloud.fmat(2,:); -% objCloud.fmat = newFmat; - -% % Create artificial bottom for glue bottle -% objLimits = [min(objCloud.vmat,[],2),max(objCloud.vmat,[],2)]; -% bottomPts = find(objCloud.vmat(3,:) < (objLimits(3,1) + 0.001)); -% bottomPts = bottomPts(randsample(size(bottomPts,2),50)); -% [triGridX,triGridY,triGridZ] = meshgrid(1:size(bottomPts,2),1:size(bottomPts,2),1:size(bottomPts,2)); -% triComb = [bottomPts(triGridX(:));bottomPts(triGridY(:));bottomPts(triGridZ(:));bottomPts(triGridX(:))]; -% objCloud.fmat = [objCloud.fmat triComb-1]; - -% Save object -% offWriter('data/models/book_joke_new.off',objCloud); -objCloud = pointCloud(objPts','Color',objCloud.Color); -pcwrite(objCloud,objName,'PLYformat','binary'); - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/flipObject.m b/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/flipObject.m deleted file mode 100644 index 602dd11..0000000 --- a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/flipObject.m +++ /dev/null @@ -1,55 +0,0 @@ - -objDir = dir('*.ply'); - -for objIdx = 1:length(objDir) - objName = objDir(objIdx).name; - objName = objName(1:(end-4)); - objCloud = pcread(sprintf('%s.ply',objName)); - objPts = objCloud.Location; - objPts(:,1) = -objPts(:,1); - objCloud = pointCloud(objPts,'Color',objCloud.Color); - pcwrite(objCloud,objName,'PLYformat','binary'); -end - -% % Load objects -% objName = 'woods_extension_cord'; -% objCloud = pcread(sprintf('%s.ply',objName)); -% -% % % Downsample object -% % objCloud = pcdownsample(objCloud,'gridAverage',0.001); -% % objCloud = pcdenoise(objCloud,'NumNeighbors',4); -% -% % Rotate with respect to PCA, major axis = x -% % coeff = pca(objCloud.vmat'); -% % objCloud.vmat = inv(coeff) * objCloud.vmat; -% objCloudDownsampled = pcdownsample(objCloud,'gridAverage',0.005); -% objPts = objCloud.Location'; -% coeff = pca(objCloudDownsampled.Location); -% objPts = inv(coeff) * objPts; -% -% % % Make z-up for now -% % objCloud.vmat = [0 0 1;0 1 0;1 0 0] * objCloud.vmat; -% -% % Center object -% objLimits = [min(objPts,[],2),max(objPts,[],2)]; -% objCenter = mean(objLimits,2); -% objPts = objPts - repmat(objCenter,1,size(objPts,2)); -% -% % % Fix normals -% % newFmat = objCloud.fmat; -% % newFmat(2,:) = objCloud.fmat(3,:); -% % newFmat(3,:) = objCloud.fmat(2,:); -% % objCloud.fmat = newFmat; -% -% % % Create artificial bottom for glue bottle -% % objLimits = [min(objCloud.vmat,[],2),max(objCloud.vmat,[],2)]; -% % bottomPts = find(objCloud.vmat(3,:) < (objLimits(3,1) + 0.001)); -% % bottomPts = bottomPts(randsample(size(bottomPts,2),50)); -% % [triGridX,triGridY,triGridZ] = meshgrid(1:size(bottomPts,2),1:size(bottomPts,2),1:size(bottomPts,2)); -% % triComb = [bottomPts(triGridX(:));bottomPts(triGridY(:));bottomPts(triGridZ(:));bottomPts(triGridX(:))]; -% % objCloud.fmat = [objCloud.fmat triComb-1]; -% -% % Save object -% % offWriter('data/models/book_joke_new.off',objCloud); -% objCloud = pointCloud(objPts','Color',objCloud.Color); -% pcwrite(objCloud,objName,'PLYformat','binary'); \ No newline at end of file diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/multiQueryKNNSearchImplGPU.m b/ros-packages/catkin_ws/src/pose_estimation/src/multiQueryKNNSearchImplGPU.m index 51c9f80..6e9b7c3 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/multiQueryKNNSearchImplGPU.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/multiQueryKNNSearchImplGPU.m @@ -1,4 +1,5 @@ function [indices,dists] = multiQueryKNNSearchImplGPU(ptCloudB,locA) +% GPU version of Matlab's multiQueryKNNSearchImpl global KNNSearchGPU diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/pcregrigidGPU.m b/ros-packages/catkin_ws/src/pose_estimation/src/pcregrigidGPU.m index d2064e1..f400d18 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/pcregrigidGPU.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/pcregrigidGPU.m @@ -1,5 +1,6 @@ function [tform, movingReg, rmse] = pcregrigidGPU(moving, fixed, varargin) -%pcregrigid Register two point clouds with ICP algorithm. +% GPU version of Matlab's pcregrigid +% pcregrigid Register two point clouds with ICP algorithm. % tform = pcregrigid(moving, fixed) returns the rigid transformation % that registers the moving point cloud with the fixed point cloud. moving and % fixed are pointCloud object. tform is an affine3d object that describes the rigid diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m b/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m index e0adc06..57f13c6 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m @@ -1,4 +1,16 @@ function resp = serviceCallback(~, reqMsg, respMsg) +% Matlab service callback function for pose_estimation +% Reads scene RGB-D data with object segmentation results, and predicts +% 6D object poses +% +% --------------------------------------------------------- +% Copyright (c) 2016, Andy Zeng +% +% This file is part of the APC Vision Toolbox and is available +% under the terms of the Simplified BSD License provided in +% LICENSE. Please retain this notice and LICENSE if you use +% this file (or any portion of it) in your project. +% --------------------------------------------------------- global visPath; global savePointCloudVis; @@ -21,6 +33,7 @@ end % Load scene point cloud +fprintf(' [Processing] Loading scene point clouds\n'); scenePointCloud = getScenePointCloud(sceneData); % Load and align empty bin/tote point cloud to observation @@ -52,7 +65,6 @@ % Do 6D pose estimation for each object and save the results objPoses = getObjectPose(scenePath,sceneData,scenePointCloud,backgroundPointCloud,extBin2Bg,objName,objModel,objNum); -% objPoses = getObjectPose(tmpDataPath,visPath,totePosePath,data,backgroundPointCloud,extBin2Bg,objName,objModel,objNum,binId,savePointClouds,saveResultImages); for duplicateIdx = 1:length(objPoses) respMsg.Objects = [respMsg.Objects; objPoses(duplicateIdx)]; end diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/startService.m b/ros-packages/catkin_ws/src/pose_estimation/src/startService.m index 2b763fb..c8c76a0 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/startService.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/startService.m @@ -11,6 +11,7 @@ % LICENSE. Please retain this notice and LICENSE if you use % this file (or any portion of it) in your project. % --------------------------------------------------------- + cd(fileparts(which('startService.m'))); % User configurations (change me) diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/visualizeResults.m b/ros-packages/catkin_ws/src/pose_estimation/src/visualizeResults.m deleted file mode 100644 index 5fa377b..0000000 --- a/ros-packages/catkin_ws/src/pose_estimation/src/visualizeResults.m +++ /dev/null @@ -1,159 +0,0 @@ -function resultImage = visualizeResults(RtPCA,latentPCA,surfaceCentroid,surfaceRange,finalRt,finalScore,scenePath,objName,instanceIdx,sceneData,objSegPts,objMasks,objModelPts) - -% if strcmp(objName,'rolodex_jumbo_pencil_cup') || - -% Show segmentation result (denoised) -if strcmp(sceneData.env,'tote') - frames = [5 14]; -else - frames = [13]; -end - -allObjImage = []; -displayMode = 'Predicted Rigid Object Pose'; -for baseFrameIdx = frames - segImage = sceneData.colorFrames{baseFrameIdx}; - extBin2Cam = inv(sceneData.extCam2World{baseFrameIdx})*sceneData.extBin2World; - if strcmp(objName,'rolodex_jumbo_pencil_cup') || strcmp(objName,'dasani_water_bottle') || strcmp(objName,'platinum_pets_dog_bowl') || strcmp(objName,'easter_turtle_sippy_cup') - segMask = double(objMasks{baseFrameIdx})*255; - else - camObjSegPts = extBin2Cam(1:3,1:3) * objSegPts + repmat(extBin2Cam(1:3,4),1,size(objSegPts,2)); - pixX = round(((camObjSegPts(1,:).*sceneData.colorK(1,1))./camObjSegPts(3,:))+sceneData.colorK(1,3)); - pixY = round(((camObjSegPts(2,:).*sceneData.colorK(2,2))./camObjSegPts(3,:))+sceneData.colorK(2,3)); - validPix = find(pixX > 0 & pixX <= 640 & pixY > 0 & pixY <= 480); - segMask = zeros(480,640); - segMask(sub2ind(size(segMask),pixY(validPix)',pixX(validPix)')) = 255; - end - - segImageR = double(segImage(:,:,1)); - segImageG = double(segImage(:,:,2)); - segImageB = double(segImage(:,:,3)); - segImageR(find(segMask)) = segImageR(find(segMask))./2; - segImageG(find(segMask)) = (segImageG(find(segMask)) + 255)./2; - segImageB(find(segMask)) = segImageB(find(segMask))./2; - segImage(:,:,1) = uint8(round(segImageR)); - segImage(:,:,2) = uint8(round(segImageG)); - segImage(:,:,3) = uint8(round(segImageB)); - - % finalRt - % objModelPts - - drawBbox = true; - drawModel = true; - - modelRt = extBin2Cam * sceneData.extWorld2Bin * finalRt; - RtPCA = sceneData.extWorld2Bin * RtPCA; - if strcmp(objName,'cherokee_easy_tee_shirt') || strcmp(objName,'kyjen_squeakin_eggs_plush_puppies') || strcmp(objName,'womens_knit_gloves') || strcmp(objName,'cloud_b_plush_bear') || ... - strcmp(objName,'scotch_bubble_mailer') - modelRt(1:3,4) = surfaceCentroid; - modelRt(1:3,1:3) = RtPCA(1:3,1:3); - - extBin2Obj = inv(sceneData.extWorld2Bin * modelRt); - objModelPts = extBin2Obj(1:3,1:3) * objSegPts + repmat(extBin2Obj(1:3,4),1,size(objSegPts,2)); - - modelRt = extBin2Cam * sceneData.extWorld2Bin * modelRt; - - drawBbox = false; - displayMode = 'Estimated Observable Surface PCA'; - - axisRange = [latentPCA(1)*50,latentPCA(2)*50,latentPCA(3)*50]; - elseif strcmp(objName,'rolodex_jumbo_pencil_cup') - drawBbox = false; - drawModel = false; - axisRange = [0.06,0.06,0.06]; - elseif strcmp(objName,'dasani_water_bottle') - drawBbox = false; - drawModel = false; - axisRange = [0.1,0.03,0.03]; - elseif strcmp(objName,'platinum_pets_dog_bowl') - drawBbox = false; - drawModel = false; - axisRange = [0.06,0.06,0.015]; - else - axisRange = [max(objModelPts(1,:)),max(objModelPts(2,:)),max(objModelPts(3,:))]; - end - - camObjModelPts = modelRt(1:3,1:3) * objModelPts + repmat(modelRt(1:3,4),1,size(objModelPts,2)); - pixX = round(((camObjModelPts(1,:).*sceneData.colorK(1,1))./camObjModelPts(3,:))+sceneData.colorK(1,3)); - pixY = round(((camObjModelPts(2,:).*sceneData.colorK(2,2))./camObjModelPts(3,:))+sceneData.colorK(2,3)); - validPix = find(pixX > 0 & pixX <= 640 & pixY > 0 & pixY <= 480); - objMask = zeros(480,640); - objMask(sub2ind(size(objMask),pixY(validPix)',pixX(validPix)')) = 1; - - objImage = sceneData.colorFrames{baseFrameIdx}; - if drawModel - objImageR = double(objImage(:,:,1)); - objImageG = double(objImage(:,:,2)); - objImageB = double(objImage(:,:,3)); - objImageR(find(objMask)) = objImageR(find(objMask))./2; - objImageG(find(objMask)) = (objImageG(find(objMask)) + 255)./2; - objImageB(find(objMask)) = objImageB(find(objMask))./2; - objImage(:,:,1) = uint8(round(objImageR)); - objImage(:,:,2) = uint8(round(objImageG)); - objImage(:,:,3) = uint8(round(objImageB)); - end - - axisPts = [[0;0;0],[axisRange(1);0;0],[0;axisRange(2);0],[0;0;axisRange(3)]]; - camAxisPts = modelRt(1:3,1:3) * axisPts + repmat(modelRt(1:3,4),1,size(axisPts,2)); - axisPixX = round(((camAxisPts(1,:).*sceneData.colorK(1,1))./camAxisPts(3,:))+sceneData.colorK(1,3)); - axisPixY = round(((camAxisPts(2,:).*sceneData.colorK(2,2))./camAxisPts(3,:))+sceneData.colorK(2,3)); - axisPts2D = [axisPixX; axisPixY]; - canvasImage = insertShape(objImage, 'line', [axisPts2D(:,1)',axisPts2D(:,2)'], 'LineWidth', 3,'Color', 'red'); - canvasImage = insertShape(canvasImage, 'line', [axisPts2D(:,1)',axisPts2D(:,3)'], 'LineWidth', 3,'Color', 'green'); - canvasImage = insertShape(canvasImage, 'line', [axisPts2D(:,1)',axisPts2D(:,4)'], 'LineWidth', 3,'Color', 'blue'); - % test = insertShape(test, 'FilledCircle', [axisPts2D(:,1)' 2], 'LineWidth', 1,'Opacity',1,'Color', 'black'); - - if drawBbox - bboxRange = [min(objModelPts(1,:)),max(objModelPts(1,:));min(objModelPts(2,:)),max(objModelPts(2,:));min(objModelPts(3,:)),max(objModelPts(3,:))]; - bboxCorners = [[bboxRange(1,1);bboxRange(2,1);bboxRange(3,1)], ... - [bboxRange(1,1);bboxRange(2,1);bboxRange(3,2)], ... - [bboxRange(1,1);bboxRange(2,2);bboxRange(3,2)], ... - [bboxRange(1,1);bboxRange(2,2);bboxRange(3,1)], ... - [bboxRange(1,2);bboxRange(2,1);bboxRange(3,1)], ... - [bboxRange(1,2);bboxRange(2,1);bboxRange(3,2)], ... - [bboxRange(1,2);bboxRange(2,2);bboxRange(3,2)], ... - [bboxRange(1,2);bboxRange(2,2);bboxRange(3,1)]]; - camCornerPts = modelRt(1:3,1:3) * bboxCorners + repmat(modelRt(1:3,4),1,size(bboxCorners,2)); - cornerPixX = round(((camCornerPts(1,:).*sceneData.colorK(1,1))./camCornerPts(3,:))+sceneData.colorK(1,3)); - cornerPixY = round(((camCornerPts(2,:).*sceneData.colorK(2,2))./camCornerPts(3,:))+sceneData.colorK(2,3)); - cornerPts2D = [cornerPixX; cornerPixY]; - if max(cornerPts2D(:)) < 1500 % to prevent bad detection to make drawing too slow - canvasImage = insertShape(canvasImage, 'line', [cornerPts2D(:,1)',cornerPts2D(:,2)';cornerPts2D(:,2)',cornerPts2D(:,3)';cornerPts2D(:,3)',cornerPts2D(:,4)';cornerPts2D(:,4)',cornerPts2D(:,1)'], 'LineWidth', 1,'Color', 'yellow'); - canvasImage = insertShape(canvasImage, 'line', [cornerPts2D(:,5)',cornerPts2D(:,6)';cornerPts2D(:,6)',cornerPts2D(:,7)';cornerPts2D(:,7)',cornerPts2D(:,8)';cornerPts2D(:,8)',cornerPts2D(:,5)'], 'LineWidth', 1,'Color', 'yellow'); - canvasImage = insertShape(canvasImage, 'line', [cornerPts2D(:,1)',cornerPts2D(:,5)';cornerPts2D(:,2)',cornerPts2D(:,6)';cornerPts2D(:,3)',cornerPts2D(:,7)';cornerPts2D(:,4)',cornerPts2D(:,8)'], 'LineWidth', 1,'Color', 'yellow'); - end - end - - if baseFrameIdx == frames(end); - segImage = insertText(segImage,[10 450],'Segmentation Results','TextColor','white','BoxColor','black'); - canvasImage = insertText(canvasImage,[10 450],sprintf('%s',displayMode),'TextColor','white','BoxColor','black'); - end - - paddedSegImage = uint8(zeros(size(segImage,1)+5,size(segImage,2)+5,size(segImage,3))); - paddedSegImage(1:480,1:640,:) = segImage; - segImage = paddedSegImage; - paddedCanvasImage = uint8(zeros(size(canvasImage,1)+5,size(canvasImage,2)+5,size(canvasImage,3))); - paddedCanvasImage(1:480,6:645,:) = canvasImage; - canvasImage = paddedCanvasImage; - - allObjImage = cat(1,allObjImage,cat(2,segImage,canvasImage)); -end - -paddedAllObjImage = uint8(zeros(size(allObjImage,1)+5,size(allObjImage,2)+10,size(allObjImage,3))); -paddedAllObjImage(6:end,6:(end-5),:) = allObjImage; -allObjImage = paddedAllObjImage; -% objImage = canvasImage; - -% resultImage = objImage; - - - -% imshow(test) - - -imwrite(allObjImage,fullfile(scenePath,strcat(objName,sprintf('.%d.result.png',instanceIdx)))); - - - -end -