Skip to content

Commit 31b83ed

Browse files
Fixing arucos
1 parent 523eb0d commit 31b83ed

34 files changed

+651
-401
lines changed

atom_calibration/scripts/calibrate

+1-1
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,7 @@ def main():
7474
ap.add_argument("-rpd", "--remove_partial_detections", help="Remove detected labels which are only partial."
7575
"Used or the Charuco.", action="store_true", default=False)
7676
ap.add_argument("-nig", "--noisy_initial_guess", nargs=2, metavar=("translation", "rotation"),
77-
help="Percentage of noise to add to the initial guess atomic transformations set before.",
77+
help="Percentage of noise to add to the initial guess atomic transformations set before starting optimization.",
7878
type=float, default=[0.0, 0.0],),
7979
ap.add_argument("-ssf", "--sensor_selection_function", default=None, type=lambda s: eval(s, globals()),
8080
help="A string to be evaluated into a lambda function that receives a sensor name as input and "

atom_calibration/scripts/deprecated/view_pattern.py

+4-6
Original file line numberDiff line numberDiff line change
@@ -2,11 +2,11 @@
22

33
import sys
44
import argparse
5+
56
import cv2
67
import rospy
78
from sensor_msgs.msg import Image
89
from cv_bridge import CvBridge
9-
1010
from atom_calibration.collect import patterns
1111

1212

@@ -37,16 +37,14 @@ def onImageReceived(self, image_msg):
3737
result = self.pattern.detect(image, equalize_histogram=False)
3838

3939

40-
41-
4240
self.pattern.drawKeypoints(image, result)
4341

4442
image_msg_out = self.bridge.cv2_to_imgmsg(image, 'passthrough')
4543
self.image_pub.publish(image_msg_out)
4644

4745
cv2.namedWindow(self.options['topic'], cv2.WINDOW_NORMAL)
4846
cv2.imshow(self.options['topic'], image)
49-
key = cv2.waitKey(1)
47+
key = cv2.waitKey(20)
5048
if key & 0xff == ord('q'):
5149
rospy.signal_shutdown(1)
5250

@@ -61,9 +59,9 @@ def main():
6159
parser.add_argument("-d", "--dict", help="Charuco Dictionary", type=str, default='DICT_5X5_100')
6260
parser.add_argument("-x", "--num_x", help="Number of features in horizontal dimension.", type=int, required=True)
6361
parser.add_argument("-y", "--num_y", help="Number of features in vertical dimension.", type=int, required=True)
64-
parser.add_argument("-L", "--length", help="Lenght of the pattern marker (e.g. square, circle).", type=float,
62+
parser.add_argument("-L", "--length", help="Length of the pattern marker (e.g. square, circle).", type=float,
6563
required=True)
66-
parser.add_argument("-l", "--inner_length", help="Lenght of inner marker (e.g. aruco marker).", type=float,
64+
parser.add_argument("-l", "--inner_length", help="Length of inner marker (e.g. aruco marker).", type=float,
6765
default=0.014)
6866
args = vars(parser.parse_args())
6967

atom_calibration/scripts/deprecated/view_pattern3d.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -163,7 +163,7 @@ def main():
163163
default=0.014)
164164
args = vars(parser.parse_args())
165165

166-
scd = SimplePatternDetector(args)
166+
SimplePatternDetector(args)
167167
rospy.spin()
168168

169169

atom_calibration/src/atom_calibration/collect/patterns.py

+2-49
Original file line numberDiff line numberDiff line change
@@ -78,18 +78,11 @@ def __init__(self, size, length, marker_length, dictionary='DICT_5X5_100'):
7878

7979
self.size = (size["x"], size["y"])
8080
self.number_of_corners = size["x"] * size["y"]
81-
# self.dictionary = cv2.aruco.getPredefinedDictionary(cdictionary)
8281

83-
# arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_5X5_100)
84-
print('Getting aruco dict')
8582
self.dictionary = cv2.aruco.Dictionary_get(cdictionary)
86-
print('Got aruco dict')
8783

88-
# self.board = cv2.aruco.CharucoBoard((size["x"] + 1, size["y"] + 1), length, marker_length, self.dictionary)
89-
# self.board = cv2.aruco.CharucoBoard((size["x"] + 1, size["y"] + 1), length, marker_length, self.dictionary)
90-
# TODO changed to CharucoBoard_create
9184
self.board = cv2.aruco.CharucoBoard_create(size["x"] + 1, size["y"] + 1, length, marker_length, self.dictionary)
92-
print(self.board)
85+
# print(self.board)
9386

9487
def detect(self, image, equalize_histogram=False):
9588

@@ -106,67 +99,27 @@ def detect(self, image, equalize_histogram=False):
10699
# params = cv2.aruco.DetectorParameters()
107100
# params = cv2.aruco.DetectorParameters()
108101

109-
print('0')
110102
params = cv2.aruco.DetectorParameters_create()
111-
112-
# print(params)
113-
# print('Line 98')
114-
115-
# setup initial data
116-
print('1')
117-
params.adaptiveThreshConstant = 2
118-
print('2')
119-
# params.adaptiveThreshWinSizeMin = 3
120-
# params.adaptiveThreshWinSizeMax = 10
121-
# params.adaptiveThreshWinSizeStep = 5
122-
params.minMarkerPerimeterRate = 0.003
123-
params.maxMarkerPerimeterRate = 4
124-
params.minCornerDistanceRate = 0.1
125-
params.markerBorderBits = 1
126-
params.minOtsuStdDev = 15
127-
params.perspectiveRemoveIgnoredMarginPerCell = .1
128-
params.maxErroneousBitsInBorderRate = .15
129-
params.errorCorrectionRate = .6
130-
131-
print('3')
132-
params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
133-
# param.doCornerRefinement = False
134-
135-
print('Line 115')
136103
corners, ids, rejected = cv2.aruco.detectMarkers(gray, self.dictionary, parameters=params)
137-
# (corners, ids, rejected) = cv2.aruco.detectMarkers(image, arucoDict,
138-
# parameters=arucoParams)
139104

140105
# print('corners = ' + str(corners))
141-
print('Line 129')
142-
# corners, ids, rejected, _ = cv2.aruco.refineDetectedMarkers(gray, self.board, corners, ids, rejected)
143-
print('4')
144106
if len(corners) > 4:
145-
print('4.5')
146107
ret, ccorners, cids = cv2.aruco.interpolateCornersCharuco(corners, ids, gray, self.board)
147-
148-
print('5')
149-
150108
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 500, 0.0001)
151109
# TODO is it 5x5 or 3x3 ...
152-
# Commented this because it return error in more recent opencv versions
153-
# ccorners = cv2.cornerSubPix(gray, ccorners, (5, 5), (-1, -1), criteria)
110+
ccorners = cv2.cornerSubPix(gray, ccorners, (5, 5), (-1, -1), criteria)
154111

155-
print('6')
156112
# A valid detection must have at least 25% of the total number of corners.
157113
detected = ccorners is not None and len(ccorners) > self.number_of_corners / 4
158114

159-
print('7')
160115
if detected:
161116
return {'detected': detected, 'keypoints': ccorners, 'ids': cids.ravel().tolist()}
162-
print('8')
163117

164118

165119
return {"detected": False, 'keypoints': np.array([]), 'ids': []}
166120

167121
def drawKeypoints(self, image, result):
168122
if result['keypoints'] is None or len(result['keypoints']) == 0:
169-
# print("none")
170123
return
171124
points = result['keypoints'].astype(np.int32)
172125
for point in points:

atom_core/src/atom_core/utilities.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -176,7 +176,7 @@ def verifyAnchoredSensor(anchored_sensor, sensors):
176176
print('Checking if anchored sensor ' + Fore.BLUE + str(anchored_sensor) + Style.RESET_ALL + ' is valid ... '+ Style.RESET_ALL, end='')
177177

178178
if anchored_sensor != '' and not anchored_sensor in list(sensors.keys()):
179-
atomError('Anchored sensor ' + Fore.BLUE + anchored_sensor + Style.RESET_ALL+ ' must be empty string or one of the configured sensors.')
179+
atomError('Anchored sensor ' + Fore.BLUE + anchored_sensor + Style.RESET_ALL+ ' must be one of the configured sensors (or an empty string).')
180180

181181
def saveFileResults(train_json, test_json, results_name, table_to_save):
182182
dataset_name = train_json.split('/')[-1].split('.')[0]

atom_examples/rgb_rgb_system/README.md

+59-31
Original file line numberDiff line numberDiff line change
@@ -42,52 +42,80 @@ This will put the bag file into your $ROS_BAGS folder.
4242

4343
# Calibration
4444

45-
...
45+
The calibration of any robotic system using **ATOM** may have several variants. We recommend a careful reading of the [documentation](https://lardemua.github.io/atom_documentation/) to learn all the details.
4646

47-
<!--
48-
## Configuring a calibration package
49-
Once your calibration package is created you will have to configure the calibration procedure by editing the softbot_calibration/calibration/config.yml file with your system information. Here is an example of a config.yml file.
50-
51-
rosrun atom_calibration create_calibration_pkg --name softbot_calibration
47+
In this section, out goal is to carry out the simplest possible calibration pipeline for the **rgb_rgb_system**.
5248

53-
After filling the config.yml file, you can run the package configuration:
49+
To calibrate, we will need a bagfile called [rgb_rgb_system_example_bag.bag](https://drive.google.com/file/d/1Noo3eZh72m-xRobYZywdo1wtqg7e4wGa/view?usp=sharing), which contains a recording of the system's data when viewing a calibration pattern in several positions.
50+
We produced the bagfile by bringing up the system and then recording a bagfile as described above.
51+
This is a small bagfile with 40 seconds / 60MB for demonstration purposes. Typically, calibration bagfiles are larger.
5452

55-
rosrun atom_calibration configure_calibration_pkg -n softbot_calibration --use_tfs
53+
Download the bagfile and put it in **$ROS_BAGS/rgb_rgb_system**.
5654

57-
This will create a set of files for launching the system, configuring rviz, etc.
55+
Next we describe each of the steps in the calibration pipeline.
5856

59-
## Collect data
57+
## Creating a calibration package
6058

61-
To run a system calibration, one requires sensor data collected at different time instants. We refer to these as data collections. To collect data, the user should launch:
59+
See also the [generic documentation](https://lardemua.github.io/atom_documentation/procedures/#create-a-calibration-package) on this topic.
6260

63-
roslaunch softbot_calibration collect_data.launch output_folder:=~/datasets/softbot/dataset3 overwrite:=true
64-
## Calibrate sensors
65-
finally run an optimization that will calibrate your sensors:
61+
Using ATOM conventions, we define name of the calibration package as **rgb_rgb_system_calibration**, and create it using:
6662

67-
roslaunch softbot_calibration calibrate.launch dataset_file:=~/datasets/softbot/dataset3/dataset.json run_calibration:=false
63+
rosrun atom_calibration create_calibration_pkg --name rgb_rgb_system_calibration
6864

69-
and then launch the script in standalone mode
65+
**NOTE**: This procedure is carried out only once, and was done already. As such, this ros package is already [included in the atom repo](https://github.com/lardemua/atom/tree/miguelriemoliveira/issue629/atom_examples/rgb_rgb_system/rgb_rgb_system_calibration). Therefore, you **should not execute this instruction** for the rgb_rgb_system.
7066

71-
rosrun atom_calibration calibrate -json ~/datasets/softbot/dataset3/dataset.json -phased -rv -v -si
72-
OBS: If needed we can exclude some of the bad collections:
7367

74-
rosrun atom_calibration calibrate -json ~/datasets/softbot/dataset4/dataset_corrected.json -phased -rv -v -si -csf "lambda x: int(x) not in [16,21,23,24,34,36]"
68+
## Configuring the calibration
7569

76-
It is possible to add an initial guess of the position of the sensors in order to get a more real result
7770

78-
rosrun atom_calibration calibrate -json ~/datasets/softbot/dataset4/dataset_corrected.json -phased -rv -v -si -csf "lambda x: int(x) not in [18,24,23] " -nig 0.01 0.003 -ss 3 -ipg
71+
This is the [config.yml](https://github.com/lardemua/atom/blob/miguelriemoliveira/issue629/atom_examples/rgb_rgb_system/rgb_rgb_system_calibration/calibration/config.yml) that we wrote to define the calibration. There are two sensors to be calibrated, named **rgb_left** and **rgb_right**. The pattern is a charuco marker.
72+
The configuration file points to the bagfile mentioned above, and the _anchored_sensor_ is defined as the **rgb_left** sensor.
7973

80-
To evaluate the calibration that was done, its need to do the annotation
74+
To configure run:
8175

82-
rosrun atom_evaluation annotate.py -test_json TEST_JSON_FILE -cs front_left_camera -si -->
76+
rosrun rgb_rgb_system_calibration configure
8377

84-
# Installation
78+
Which will run a series of checks and produce several files inside the **rgb_rgb_system_calibration** package.
8579

86-
##### Add to .bashrc:
87-
```
88-
export TURTLEBOT3_MODEL="waffle"
89-
export ROS_BAGS="/home/<username>/bagfiles"
90-
export ATOM_DATASETS="/home/<username>/datasets"
91-
export GAZEBO_MODEL_PATH="`rospack find softbot_gazebo`/models:${GAZEBO_MODEL_PATH}"
92-
```
9380

81+
## Collecting a dataset
82+
83+
To collect a dataset we run:
84+
85+
roslaunch rgb_rgb_system_calibration collect_data.launch output_folder:=$ATOM_DATASETS/rgb_rgb_system/rgb_rgb_system_example overwrite:=true
86+
87+
And save a few collections.
88+
89+
We will use as example the [rgb_rgb_system_example_dataset](https://drive.google.com/file/d/1i4cOp1BcPsifmr2VLpYM0mYa0_SWaUGK/view?usp=sharing), which contains 4 collections, as shown bellow.
90+
91+
Download and decompress the dataset to **$ATOM_DATASETS/rgb_rgb_system/rgb_rgb_system_example_dataset**.
92+
93+
Collection | rgb_left | rgb_right
94+
:----------------:|:-------------------------:|:-------------------------:
95+
0 | ![](docs/rgb_left_000.jpg) | ![](docs/rgb_right_000.jpg)
96+
1 | ![](docs/rgb_left_001.jpg) | ![](docs/rgb_right_001.jpg)
97+
2 | ![](docs/rgb_left_002.jpg) | ![](docs/rgb_right_002.jpg)
98+
3 | ![](docs/rgb_left_003.jpg) | ![](docs/rgb_right_003.jpg)
99+
100+
101+
## Calibrate
102+
103+
To calibrate, first setup visualization with:
104+
105+
roslaunch rgb_rgb_system_calibration calibrate.launch
106+
107+
Then carry out the actual calibration using:
108+
109+
rosrun atom_calibration calibrate -json $ATOM_DATASETS/rgb_rgb_system/rgb_rgb_system_example_dataset/dataset.json -v -rv
110+
111+
This will produce a table of residuals per iteration, like this:
112+
113+
![](docs/calibration_output.png)
114+
115+
This is the table presented once calibration is complete, which shows average reprojection errors of under 1 pixel. Sub-pixel accuracy is considered a good result for rgb camera calibration.
116+
117+
Since this is a simulation, the original pose of the cameras is actually the one used by gazebo to produce the images. This means that the cameras are already positioned in the actual ground truth pose, which means that the calibration did not do much in this case. In a real system, the sensors will not be positioned at the ground truth pose. In fact, for real systems, we do not know where the ground truth is.
118+
119+
To make sure this ATOM is actually calibrating sensor poses in simulated experiments, we use the --noise_initial_guess (-nig) flag. This makes the calibrate script add a random variation to the initial pose of the cameras, to be sure they are not located at the ground truth:
120+
121+
rosrun atom_calibration calibrate -json $ATOM_DATASETS/rgb_rgb_system/rgb_rgb_system_example_dataset/dataset.json -v -rv -nig
Loading
Loading
Loading
Loading
Loading
Loading
Loading
Loading
Loading

atom_examples/rgb_rgb_system/rgb_rgb_system_bringup/config/config.rviz

+15-14
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ Panels:
66
Expanded:
77
- /Global Options1
88
- /Status1
9+
- /InteractiveMarkers1
910
- /TF1/Frames1
1011
Splitter Ratio: 0.5
1112
Tree Height: 728
@@ -110,7 +111,7 @@ Visualization Manager:
110111
Value: true
111112
Visual Enabled: true
112113
- Class: rviz/Image
113-
Enabled: true
114+
Enabled: false
114115
Image Topic: /rgb_right/image_raw
115116
Max Value: 1
116117
Median window: 5
@@ -120,9 +121,9 @@ Visualization Manager:
120121
Queue Size: 2
121122
Transport Hint: raw
122123
Unreliable: false
123-
Value: true
124+
Value: false
124125
- Class: rviz/Image
125-
Enabled: true
126+
Enabled: false
126127
Image Topic: /rgb_left/image_raw
127128
Max Value: 1
128129
Median window: 5
@@ -132,14 +133,14 @@ Visualization Manager:
132133
Queue Size: 2
133134
Transport Hint: raw
134135
Unreliable: false
135-
Value: true
136+
Value: false
136137
- Class: rviz/InteractiveMarkers
137138
Enable Transparency: true
138139
Enabled: true
139140
Name: InteractiveMarkers
140-
Show Axes: false
141+
Show Axes: true
141142
Show Descriptions: true
142-
Show Visual Aids: false
143+
Show Visual Aids: true
143144
Update Topic: /interactive_pattern/update
144145
Value: true
145146
- Class: rviz/TF
@@ -213,33 +214,33 @@ Visualization Manager:
213214
Views:
214215
Current:
215216
Class: rviz/Orbit
216-
Distance: 0.8620563745498657
217+
Distance: 2.5529768466949463
217218
Enable Stereo Rendering:
218219
Stereo Eye Separation: 0.05999999865889549
219220
Stereo Focal Distance: 1
220221
Swap Stereo Eyes: false
221222
Value: false
222223
Field of View: 0.7853981852531433
223224
Focal Point:
224-
X: 0.010522996075451374
225-
Y: 0.003330834675580263
226-
Z: 1.6504679918289185
225+
X: 1.1224764585494995
226+
Y: 0.21075978875160217
227+
Z: 1.3394124507904053
227228
Focal Shape Fixed Size: true
228229
Focal Shape Size: 0.05000000074505806
229230
Invert Z Axis: false
230231
Name: Current View
231232
Near Clip Distance: 0.009999999776482582
232-
Pitch: 0.6153982281684875
233+
Pitch: 0.1503986269235611
233234
Target Frame: <Fixed Frame>
234-
Yaw: 1.0303995609283447
235+
Yaw: 4.155409336090088
235236
Saved: ~
236237
Window Geometry:
237238
Displays:
238239
collapsed: false
239240
Height: 1022
240241
Hide Left Dock: false
241242
Hide Right Dock: false
242-
QMainWindow State: 000000ff00000000fd00000004000000000000023900000362fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c00000362000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a300000362fc0200000005fb00000010007200670062005f006c006500660074010000003c000001af0000001600fffffffb00000012007200670062005f0072006900670068007401000001f1000001ad0000001600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003c000002b2000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000029800fffffffb0000000800540069006d00650100000000000004500000000000000000000003980000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
243+
QMainWindow State: 000000ff00000000fd00000004000000000000015600000362fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c00000362000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000017f00000362fc0200000005fb00000010007200670062005f006c006500660074000000003c000003620000001600fffffffb00000012007200670062005f0072006900670068007400000001fa000001a40000001600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003c000002b2000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004600000003efc0100000002fb0000000800540069006d00650100000000000004600000029800fffffffb0000000800540069006d00650100000000000004500000000000000000000003040000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
243244
Selection:
244245
collapsed: false
245246
Time:
@@ -248,7 +249,7 @@ Window Geometry:
248249
collapsed: false
249250
Views:
250251
collapsed: false
251-
Width: 1920
252+
Width: 1120
252253
X: 0
253254
Y: 30
254255
rgb_left:

0 commit comments

Comments
 (0)