help='Number of timesteps between each snapshot.')
parser.add_argument('--headless',
action='store_true',
default=False,
help='If true, uses headless rendering.')
parser.add_argument('--debug', action='store_true', default=False)
if __name__ == '__main__':
args = parser.parse_args()
info = preload_object(args.object)
print("Preloaded the object.")
# setup simulator
setup_pybullet(time_step=config.TIME_STEP,
renders=not args.headless,
gravity=True)
sensor = make_sensor(size=[1.5, 1.5, 1],
position=[0, 0, 0.5],
sensor_vector=[0, 0, 1],
thickness=0.01,
use_force=False,
constrained=False)
img_counter = 0
# set initial position and orientation
position = np.array([0., 0., 1.3])
orientation = np.array([0, 0, 0, 1])
base_pose = geometry.list2pose_stamped(list(position) + list(orientation))
T = transformations.euler_matrix(0, 0, 0)
x_world_1 = np.reshape(x_world_1, (-1, 1))
x_cam = self.cam.project_3D_to_pixel(x_world_1)
x_world_2 = self.cam.unproject_pixel_to_3D(x_cam)
np.testing.assert_array_almost_equal(x_world_1, x_world_2)
def test_pointcloud(self):
"""
Tests pointcloud-to-depthimage and depthimage-to-pointcloud
and consistency of the `Camera` with PyBullet and OpenGL camera.
"""
rgb_img_1, depth_img_1, seg_img_1 = self.cam.get_pybullet_image()
pcl_points, pcl_colors = self.cam.unproject_canvas_to_pointcloud(rgb_img_1, depth_img_1)
rgb_img_2, depth_img_2 = self.cam.project_pointcloud_to_canvas(pcl_points, pcl_colors)
# show the depth images
self.cam.show_image(depth_img_1, RGB=False, title="Image 1")
self.cam.show_image(depth_img_2, RGB=False, title="Image 2")
np.testing.assert_array_almost_equal(depth_img_1, depth_img_2)
np.testing.assert_array_almost_equal(rgb_img_1, rgb_img_2)
if __name__ == '__main__':
setup_pybullet(time_step=TIME_STEP)
suite = unittest.TestSuite()
suite.addTest(TestCamera('test_camera'))
suite.addTest(TestCamera('test_pointcloud'))
unittest.TextTestRunner(verbosity=2).run(suite)