| 386 | } |
| 387 | |
| 388 | void sloam::computeModels(SloamInput &in, std::vector<Cylinder> &landmarks, std::vector<Plane> &planes) |
| 389 | { |
| 390 | boost::multi_array<VectorType, 2> scgf( |
| 391 | boost::extents[fmParams_.groundRadiiBins][fmParams_.groundThetaBins]); |
| 392 | binGroundPoints(SE3(), in.groundCloud->points, scgf); |
| 393 | |
| 394 | Vector3 src_vec(0, 0, 1); |
| 395 | Matrix4 tfm = in.poseEstimate.inverse().matrix().transpose(); |
| 396 | for (auto r = 0; r < fmParams_.groundRadiiBins; ++r) |
| 397 | { |
| 398 | for (auto t = 0; t < fmParams_.groundThetaBins; ++t) |
| 399 | { |
| 400 | Plane ground(scgf[r][t], fmParams_); |
| 401 | // check if model is roughly consistent with what the ground should look like |
| 402 | auto normal = tfm * ground.model.plane; |
| 403 | Quat q_rot = Quat::FromTwoVectors(src_vec, normal.segment(0, 3)); |
| 404 | auto angles = q_rot.toRotationMatrix().eulerAngles(0, 1, 2); |
| 405 | bool angleCheck = (angles[0] < 0.1 && angles[1] < 0.1 && angles[2] < 0.1) || |
| 406 | (M_PI - abs(angles[0]) < 0.1 && M_PI - abs(angles[1]) < 0.1 && M_PI - abs(angles[2]) < 0.1); |
| 407 | // ground should be under the robot |
| 408 | bool heightCheck = (in.poseEstimate * ground.model.centroid)(2) < in.poseEstimate.translation()(2); |
| 409 | if (ground.isValid && angleCheck && heightCheck) |
| 410 | planes.push_back(ground); |
| 411 | } |
| 412 | } |
| 413 | |
| 414 | if (planes.size() == 0) |
| 415 | return; |
| 416 | |
| 417 | // landmarks |
| 418 | for (const std::vector<TreeVertex> t : in.landmarks) |
| 419 | { |
| 420 | auto approxTreePos = t[1].coords; |
| 421 | Scalar bestDist = 100000; |
| 422 | Plane bestPlane = planes[0]; |
| 423 | for (auto &ground : planes) |
| 424 | { |
| 425 | Scalar d = ground.distance(approxTreePos); |
| 426 | if (d < bestDist) |
| 427 | { |
| 428 | bestDist = d; |
| 429 | bestPlane = ground; |
| 430 | } |
| 431 | } |
| 432 | auto c = Cylinder(t, bestPlane, fmParams_); |
| 433 | if (c.isValid) |
| 434 | landmarks.push_back(c); |
| 435 | } |
| 436 | } |
| 437 | |
| 438 | void sloam::projectModels(const SE3 &tf, std::vector<Cylinder> &landmarks, std::vector<Plane> &planes) |
| 439 | { |