| 226 | } |
| 227 | |
| 228 | void init(ros::NodeHandle& nh) { |
| 229 | // set parameters of mapping |
| 230 | // cam2body_R_ << 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0; |
| 231 | // cam2body_p_.setZero(); |
| 232 | std::vector<double> tmp; |
| 233 | if (nh.param<std::vector<double>>("cam2body_R", tmp, std::vector<double>())) { |
| 234 | cam2body_R_ = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(tmp.data(), 3, 3); |
| 235 | } |
| 236 | if (nh.param<std::vector<double>>("cam2body_p", tmp, std::vector<double>())) { |
| 237 | cam2body_p_ = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(tmp.data(), 3, 1); |
| 238 | } |
| 239 | // std::cout << "R: \n" << cam2body_R_ << std::endl; |
| 240 | // std::cout << "p: \n" << cam2body_p_ << std::endl; |
| 241 | double res; |
| 242 | Eigen::Vector3d map_size; |
| 243 | // NOTE whether to use global map |
| 244 | nh.getParam("use_global_map", use_global_map_); |
| 245 | if (use_global_map_) { |
| 246 | double x, y, z, res; |
| 247 | nh.getParam("x_length", x); |
| 248 | nh.getParam("y_length", y); |
| 249 | nh.getParam("z_length", z); |
| 250 | nh.getParam("resolution", res); |
| 251 | nh.getParam("inflate_size", inflate_size_); |
| 252 | gridmap_.setup(res, Eigen::Vector3d(x, y, z), 10, true); |
| 253 | } else { |
| 254 | // camera parameters |
| 255 | nh.getParam("camera_rate", camConfig_.rate); |
| 256 | nh.getParam("camera_range", camConfig_.range); |
| 257 | nh.getParam("cam_width", camConfig_.width); |
| 258 | nh.getParam("cam_height", camConfig_.height); |
| 259 | nh.getParam("cam_fx", camConfig_.fx); |
| 260 | nh.getParam("cam_fy", camConfig_.fy); |
| 261 | nh.getParam("cam_cx", camConfig_.cx); |
| 262 | nh.getParam("cam_cy", camConfig_.cy); |
| 263 | nh.getParam("depth_scaling_factor", camConfig_.depth_scaling_factor); |
| 264 | // mapping parameters |
| 265 | nh.getParam("down_sample_factor", down_sample_factor_); |
| 266 | nh.getParam("resolution", res); |
| 267 | nh.getParam("local_x", map_size.x()); |
| 268 | nh.getParam("local_y", map_size.y()); |
| 269 | nh.getParam("local_z", map_size.z()); |
| 270 | nh.getParam("inflate_size", inflate_size_); |
| 271 | gridmap_.setup(res, map_size, camConfig_.range); |
| 272 | // depth filter parameters |
| 273 | nh.getParam("depth_filter_tolerance", depth_filter_tolerance_); |
| 274 | nh.getParam("depth_filter_mindist", depth_filter_mindist_); |
| 275 | nh.getParam("depth_filter_margin", depth_filter_margin_); |
| 276 | // raycasting parameters |
| 277 | int p_min, p_max, p_hit, p_mis, p_occ, p_def; |
| 278 | nh.getParam("p_min", p_min); |
| 279 | nh.getParam("p_max", p_max); |
| 280 | nh.getParam("p_hit", p_hit); |
| 281 | nh.getParam("p_mis", p_mis); |
| 282 | nh.getParam("p_occ", p_occ); |
| 283 | nh.getParam("p_def", p_def); |
| 284 | gridmap_.setupP(p_min, p_max, p_hit, p_mis, p_occ, p_def); |
| 285 | } |