error_block_tests.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Fetch Robotics Inc.
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <urdf/model.h>
18 #include <robot_calibration/ceres/calibration_data_helpers.h>
19 #include <robot_calibration/ceres/optimizer.h>
20 #include <robot_calibration/ceres/chain3d_to_mesh_error.h>
21 #include <gtest/gtest.h>
22 
23 std::string robot_description =
24 "<?xml version='1.0' ?>"
25 "<robot name='maxwell'>"
26 " <link name='base_link'/>"
27 " <joint name='torso_joint' type='fixed'>"
28 " <origin rpy='0 0 0' xyz='-0.00635 0 0.7914'/>"
29 " <parent link='base_link'/>"
30 " <child link='torso_link'/>"
31 " </joint>"
32 " <link name='torso_link'/>"
33 " <joint name='torso_actuator_joint' type='fixed'>"
34 " <origin rpy='0 0 0' xyz='0 0 0'/>"
35 " <parent link='torso_link'/>"
36 " <child link='torso_actuator_link'/>"
37 " </joint>"
38 " <link name='torso_actuator_link'/>"
39 " <joint name='arm_lift_joint' type='prismatic'>"
40 " <axis xyz='0 0 1'/>"
41 " <limit effort='30' lower='-0.464' upper='0' velocity='0.0508'/>"
42 " <origin rpy='0 0 0' xyz='0 0 0'/>"
43 " <parent link='torso_link'/>"
44 " <child link='arm_lift_link'/>"
45 " </joint>"
46 " <link name='arm_lift_link'/>"
47 " <joint name='arm_base_joint' type='fixed'>"
48 " <origin rpy='0 0 0' xyz='0.0611 0 0'/>"
49 " <parent link='arm_lift_link'/>"
50 " <child link='arm_link'/>"
51 " </joint>"
52 " <link name='arm_link'/>"
53 " <joint name='arm_shoulder_pan_servo_joint' type='fixed'>"
54 " <origin rpy='0 0 0' xyz='0 0 0'/>"
55 " <parent link='arm_link'/>"
56 " <child link='arm_shoulder_pan_servo_link'/>"
57 " </joint>"
58 " <link name='arm_shoulder_pan_servo_link'/>"
59 " <joint name='arm_shoulder_pan_joint' type='revolute'>"
60 " <origin rpy='0 0 0' xyz='0 0 0.0235'/>"
61 " <axis xyz='0 0 1'/>"
62 " <limit effort='30' lower='-1.57' upper='1.57' velocity='0.524'/>"
63 " <parent link='arm_shoulder_pan_servo_link'/>"
64 " <child link='arm_shoulder_pan_link'/>"
65 " </joint>"
66 " <link name='arm_shoulder_pan_link'/>"
67 " <joint name='arm_shoulder_lift_servo_joint' type='fixed'>"
68 " <origin rpy='0 -1.57 0' xyz='0 0 0.0526'/>"
69 " <parent link='arm_shoulder_pan_link'/>"
70 " <child link='arm_shoulder_lift_servo_link'/>"
71 " </joint>"
72 " <link name='arm_shoulder_lift_servo_link'/>"
73 " <joint name='arm_shoulder_lift_joint' type='revolute'>"
74 " <origin rpy='0 1.57 0' xyz='0 0 0'/>"
75 " <axis xyz='0 1 0'/>"
76 " <limit effort='30' lower='-1.77' upper='1.317' velocity='0.524'/>"
77 " <parent link='arm_shoulder_lift_servo_link'/>"
78 " <child link='arm_shoulder_lift_link'/>"
79 " </joint>"
80 " <link name='arm_shoulder_lift_link'/>"
81 " <joint name='arm_upperarm_roll_servo_joint' type='fixed'>"
82 " <origin rpy='1.57 1.57 0' xyz='0.0712978 0 0'/>"
83 " <parent link='arm_shoulder_lift_link'/>"
84 " <child link='arm_upperarm_roll_servo_link'/>"
85 " </joint>"
86 " <link name='arm_upperarm_roll_servo_link'/>"
87 " <joint name='arm_upperarm_roll_joint' type='revolute'>"
88 " <origin rpy='-1.57 0 1.57' xyz='0 0 0'/>"
89 " <axis xyz='1 0 0'/>"
90 " <limit effort='30' lower='-2' upper='2' velocity='0.524'/>"
91 " <parent link='arm_upperarm_roll_servo_link'/>"
92 " <child link='arm_upperarm_roll_link'/>"
93 " </joint>"
94 " <link name='arm_upperarm_roll_link'/>"
95 " <joint name='arm_elbow_flex_joint' type='revolute'>"
96 " <origin rpy='0 0 0' xyz='0.0869955 0 0'/>"
97 " <axis xyz='0 1 0'/>"
98 " <limit effort='30' lower='-1.57' upper='2.617' velocity='0.524'/>"
99 " <parent link='arm_upperarm_roll_link'/>"
100 " <child link='arm_elbow_flex_link'/>"
101 " </joint>"
102 " <link name='arm_elbow_flex_link'/>"
103 " <joint name='arm_forearm_fixed_joint' type='fixed'>"
104 " <origin rpy='0 0 0' xyz='0 0 0'/>"
105 " <parent link='arm_elbow_flex_link'/>"
106 " <child link='arm_forearm_link'/>"
107 " </joint>"
108 " <link name='arm_forearm_link'/>"
109 " <joint name='arm_wrist_flex_servo_joint' type='fixed'>"
110 " <origin rpy='0 0 0' xyz='0.125 0 0'/>"
111 " <parent link='arm_forearm_link'/>"
112 " <child link='arm_wrist_flex_servo_link'/>"
113 " </joint>"
114 " <link name='arm_wrist_flex_servo_link'/>"
115 " <joint name='arm_wrist_flex_joint' type='revolute'>"
116 " <origin rpy='0 0 0' xyz='0 0 0.0'/>"
117 " <axis xyz='0 1 0'/>"
118 " <limit effort='30' lower='-1.57' upper='1.57' velocity='0.785'/>"
119 " <parent link='arm_wrist_flex_servo_link'/>"
120 " <child link='arm_wrist_flex_link'/>"
121 " </joint>"
122 " <link name='arm_wrist_flex_link'/>"
123 " <joint name='arm_wrist_roll_joint' type='revolute'>"
124 " <axis xyz='1 0 0'/>"
125 " <limit effort='30' lower='-2.617' upper='2.617' velocity='0.785'/>"
126 " <origin rpy='0 0 0' xyz='0.031 0 0'/>"
127 " <parent link='arm_wrist_flex_link'/>"
128 " <child link='arm_wrist_roll_link'/>"
129 " </joint>"
130 " <link name='arm_wrist_roll_link'/>"
131 " <joint name='gripper_joint' type='fixed'>"
132 " <axis xyz='0 0 1'/>"
133 " <origin rpy='0 0 0' xyz='0.15 0 -0.015'/>"
134 " <parent link='arm_wrist_roll_link'/>"
135 " <child link='gripper_led_frame'/>"
136 " </joint>"
137 " <link name='gripper_led_frame'/>"
138 " <joint name='head_joint' type='fixed'>"
139 " <origin rpy='0 0 0' xyz='0 0 0.512375'/>"
140 " <parent link='torso_link'/>"
141 " <child link='head_link'/>"
142 " </joint>"
143 " <link name='head_link'/>"
144 " <joint name='head_pan_servo_joint' type='fixed'>"
145 " <origin rpy='0 0 0' xyz='0.010 0 0.0254'/>"
146 " <parent link='head_link'/>"
147 " <child link='head_pan_servo_link'/>"
148 " </joint>"
149 " <link name='head_pan_servo_link'/>"
150 " <joint name='head_pan_joint' type='revolute'>"
151 " <origin rpy='0 0 0' xyz='0 0 0.019'/>"
152 " <axis xyz='0 0 1'/>"
153 " <limit effort='30' lower='-2.617' upper='2.617' velocity='1.0'/>"
154 " <parent link='head_pan_servo_link'/>"
155 " <child link='head_pan_link'/>"
156 " </joint>"
157 " <link name='head_pan_link'/>"
158 " <joint name='head_tilt_servo_joint' type='fixed'>"
159 " <origin rpy='0 0 0' xyz='0 0 0.0415'/>"
160 " <parent link='head_pan_link'/>"
161 " <child link='head_tilt_servo_link'/>"
162 " </joint>"
163 " <link name='head_tilt_servo_link'/>"
164 " <joint name='head_tilt_joint' type='revolute'>"
165 " <origin rpy='0 0 0' xyz='0 0 0'/>"
166 " <axis xyz='0 1 0'/>"
167 " <limit effort='30' lower='-1.57' upper='1.57' velocity='1.0'/>"
168 " <parent link='head_tilt_servo_link'/>"
169 " <child link='head_tilt_link'/>"
170 " </joint>"
171 " <link name='head_tilt_link'/>"
172 " <joint name='head_camera_frame_joint' type='fixed'>"
173 " <origin rpy='0 0 0' xyz='0 0 0.026'/>"
174 " <parent link='head_tilt_link'/>"
175 " <child link='head_camera_frame'/>"
176 " </joint>"
177 " <link name='head_camera_frame'/>"
178 " <joint name='head_camera_ir_joint' type='fixed'>"
179 " <origin rpy='0 0 0' xyz='0.01905 -0.0269875 0.032075'/>"
180 " <parent link='head_camera_frame'/>"
181 " <child link='head_camera_ir_link'/>"
182 " </joint>"
183 " <link name='head_camera_ir_link'/>"
184 " <joint name='head_camera_ir_optical_frame_joint' type='fixed'>"
185 " <origin rpy='-1.57 0.0 -1.57' xyz='0 0 0'/>"
186 " <parent link='head_camera_ir_link'/>"
187 " <child link='head_camera_ir_optical_frame'/>"
188 " </joint>"
189 " <link name='head_camera_ir_optical_frame'/>"
190 " <joint name='head_camera_rgb_joint' type='fixed'>"
191 " <origin rpy='0 -0.08 0' xyz='0 0.0552875 0'/>"
192 " <parent link='head_camera_ir_link'/>"
193 " <child link='head_camera_rgb_link'/>"
194 " </joint>"
195 " <link name='head_camera_rgb_link'/>"
196 " <joint name='head_camera_rgb_optical_frame_joint' type='fixed'>"
197 " <origin rpy='-1.57 0.0 -1.57' xyz='0 0 0'/>"
198 " <parent link='head_camera_rgb_link'/>"
199 " <child link='head_camera_rgb_optical_frame'/>"
200 " </joint>"
201 " <link name='head_camera_rgb_optical_frame'/>"
202 "</robot>";
203 
204 TEST(ErrorBlockTests, error_blocks_maxwell)
205 {
206  ros::NodeHandle nh("~");
207 
208  robot_calibration::Optimizer opt(robot_description);
209 
210  std::vector<robot_calibration_msgs::CalibrationData> data;
211  robot_calibration_msgs::CalibrationData msg;
212 
213  // Match expected output from chain manager
214  msg.joint_states.name.resize(10);
215  msg.joint_states.name[0] = "arm_lift_joint";
216  msg.joint_states.name[1] = "arm_shoulder_pan_joint";
217  msg.joint_states.name[2] = "arm_shoulder_lift_joint";
218  msg.joint_states.name[3] = "arm_upperarm_roll_joint";
219  msg.joint_states.name[4] = "arm_elbow_flex_joint";
220  msg.joint_states.name[5] = "arm_wrist_flex_joint";
221  msg.joint_states.name[6] = "arm_wrist_roll_joint";
222  msg.joint_states.name[7] = "head_pan_joint";
223  msg.joint_states.name[8] = "head_tilt_joint";
224  msg.joint_states.name[9] = "arm_lift_joint";
225  msg.joint_states.position.resize(10);
226  msg.joint_states.position[0] = 0.0;
227  msg.joint_states.position[1] = -0.814830;
228  msg.joint_states.position[2] = -0.00022290000000002586;
229  msg.joint_states.position[3] = 0.0;
230  msg.joint_states.position[4] = -0.7087341;
231  msg.joint_states.position[5] = 0.0;
232  msg.joint_states.position[6] = 0.0;
233  msg.joint_states.position[7] = -0.8280187999999999;
234  msg.joint_states.position[8] = 0.6358500000000002;
235  msg.joint_states.position[9] = 0.0;
236 
237  // Expectect output from led finder
238  msg.observations.resize(2);
239  msg.observations[0].sensor_name = "camera";
240  msg.observations[1].sensor_name = "arm";
241 
242  msg.observations[0].features.resize(1);
243  msg.observations[0].features[0].header.frame_id = "head_camera_rgb_optical_frame";
244  msg.observations[0].features[0].point.x = -0.0143163670728;
245  msg.observations[0].features[0].point.y = 0.111304592065;
246  msg.observations[0].features[0].point.z = 0.522079317365;
247 
248  msg.observations[0].ext_camera_info.camera_info.P[0] = 100.0; // fx
249  msg.observations[0].ext_camera_info.camera_info.P[5] = 100.0; // fy
250  msg.observations[0].ext_camera_info.camera_info.P[2] = 320.0; // cx
251  msg.observations[0].ext_camera_info.camera_info.P[6] = 240.0; // cy
252  msg.observations[0].ext_camera_info.parameters.resize(2);
253  msg.observations[0].ext_camera_info.parameters[0].name = "z_offset";
254  msg.observations[0].ext_camera_info.parameters[0].value = 0.0;
255  msg.observations[0].ext_camera_info.parameters[1].name = "z_scaling";
256  msg.observations[0].ext_camera_info.parameters[1].value = 1.0;
257 
258  msg.observations[1].features.resize(1);
259  msg.observations[1].features[0].header.frame_id = "gripper_led_frame";
260  msg.observations[1].features[0].point.x = 0.0;
261  msg.observations[1].features[0].point.y = 0.0;
262  msg.observations[1].features[0].point.z = 0.0;
263 
264  // Add first data point
265  data.push_back(msg);
266 
267  // Add a second data point that is just a little different
268  msg.joint_states.position[1] = -0.019781999999999966;
269  msg.joint_states.position[7] = 0.0;
270  msg.observations[0].features[0].point.x = 0.0365330705881;
271  msg.observations[0].features[0].point.y = 0.102609552493;
272  msg.observations[0].features[0].point.z = 0.536061220027;
273  data.push_back(msg);
274 
275  // And a third data point
276  msg.joint_states.position[1] = 0.883596;
277  msg.joint_states.position[7] = 0.9442135999999999;
278  msg.observations[0].features[0].point.x = 0.0942445346646;
279  msg.observations[0].features[0].point.y = 0.11409172323;
280  msg.observations[0].features[0].point.z = 0.517497963716;
281  data.push_back(msg);
282 
283  // Test some helpers
284  EXPECT_EQ(0, robot_calibration::getSensorIndex(msg, "camera"));
285  EXPECT_EQ(-1, robot_calibration::getSensorIndex(msg, "camera2"));
286 
287  // Setup params
288  robot_calibration::OptimizationParams params;
289  params.LoadFromROS(nh);
290 
291  // Optimize
292  opt.optimize(params, data, true);
293  EXPECT_DOUBLE_EQ(1.6771013673719808e-25, opt.summary()->initial_cost);
294  // 14 joints + 6 from a free frame
295  EXPECT_EQ(20, opt.getNumParameters());
296  // 3 CalibrationData, each with chain3d with a single observed point (3 residuals)
297  EXPECT_EQ(30, opt.getNumResiduals());
298 
299  // While things are setup, test our param helpers
300  // This param does not exist, we should get the default
301  double test = params.getParam(params.error_blocks[1], "test", 10.0);
302  EXPECT_EQ(10, test);
303  // This does exist, we should get what is in our YAML file
304  double scale = params.getParam(params.error_blocks[1], "joint_scale", 10.0);
305  EXPECT_EQ(0.0, scale);
306 
307  // Validate getCameraNames()
308  std::vector<std::string> camera_names = opt.getCameraNames();
309  EXPECT_EQ(1, static_cast<int>(camera_names.size()));
310  EXPECT_EQ("camera", camera_names.front());
311 
312  // Validate distToLine()
313  Eigen::Vector3d A(0, 0, 0);
314  Eigen::Vector3d B(5, 0, 0);
315 
316  // C is below A
317  Eigen::Vector3d C(-1, 1, 0);
318  EXPECT_EQ(2, robot_calibration::distToLine(A, B, C));
319 
320  // C is above B
321  C[0] = 6;
322  EXPECT_EQ(2, robot_calibration::distToLine(A, B, C));
323 
324  // C projects
325  C[0] = 2;
326  EXPECT_EQ(1, robot_calibration::distToLine(A, B, C));
327 }
328 
329 int main(int argc, char** argv)
330 {
331  ros::init(argc, argv, "error_block_tests");
332  testing::InitGoogleTest(&argc, argv);
333  return RUN_ALL_TESTS();
334 }
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
robot_calibration::Optimizer::getNumResiduals
int getNumResiduals()
Definition: optimizer.h:78
robot_calibration::OptimizationParams::getParam
T getParam(Params &params, const std::string &name, T default_value)
Definition: optimization_params.h:74
robot_calibration::Optimizer::summary
boost::shared_ptr< ceres::Solver::Summary > summary()
Returns the summary of the optimization last run.
Definition: optimizer.h:63
TEST
TEST(ErrorBlockTests, error_blocks_maxwell)
Definition: error_block_tests.cpp:204
robot_calibration::Optimizer::getCameraNames
std::vector< std::string > getCameraNames()
Get the names of all camera models.
Definition: optimizer.cpp:399
test
robot_calibration::Optimizer::optimize
int optimize(OptimizationParams &params, std::vector< robot_calibration_msgs::CalibrationData > data, bool progress_to_stdout=false)
Run optimization.
Definition: optimizer.cpp:63
robot_calibration::B
const unsigned B
Definition: led_finder.cpp:37
A
robot_calibration::Optimizer
Class to do optimization.
Definition: optimizer.h:42
robot_calibration::OptimizationParams::LoadFromROS
bool LoadFromROS(ros::NodeHandle &nh)
Definition: optimization_params.cpp:30
robot_description
std::string robot_description
Definition: error_block_tests.cpp:23
model.h
chain3d_to_mesh_error.h
main
int main(int argc, char **argv)
Definition: error_block_tests.cpp:329
robot_calibration::distToLine
double distToLine(Eigen::Vector3d &a, Eigen::Vector3d &b, Eigen::Vector3d c)
Get the squared distance line segment A-B for point C.
Definition: chain3d_to_mesh_error.h:44
robot_calibration::OptimizationParams
Class to hold parameters for optimization.
Definition: optimization_params.h:29
optimizer.h
robot_calibration::OptimizationParams::error_blocks
std::vector< Params > error_blocks
Definition: optimization_params.h:65
calibration_data_helpers.h
robot_calibration::getSensorIndex
int getSensorIndex(const robot_calibration_msgs::CalibrationData &msg, const std::string &sensor)
Determine which observation index corresponds to a particular sensor name.
Definition: calibration_data_helpers.h:30
robot_calibration::Optimizer::getNumParameters
int getNumParameters()
Definition: optimizer.h:73
EXPECT_EQ
#define EXPECT_EQ(a, b)
ros::NodeHandle

robot_calibration
Author(s): Michael Ferguson
autogenerated on Fri Sep 1 2023 02:52:01