init rp search

libs/localize/voxel_map/fast_voxel_matcher.cc

@@ -7,7 +7,9 @@
 namespace juefx {
 
 struct DiscreteScan {
-    int32_t index;
+    int32_t yaw_index;
+    int32_t pitch_index;
+    int32_t roll_index;
     uint32_t ground_size;
     uint32_t above_ground_size;
     Rigid3f pose;
@@ -126,7 +128,7 @@ FastVoxelMatcher::Match(const Rigid3f &init_pose,
         voxel_map, lowest_resolution_candidates, param, options_.accepted_score);
     LOG(INFO) << "best_candidate.score: " << best_candidate.score;
     if (best_candidate.score > options_.accepted_score) {
-        LOG(INFO) << "index: " << best_candidate.scan->index
+        LOG(INFO) << "index: " << best_candidate.scan->yaw_index
                   << " discrete_scans.size(): " << discrete_scans.size()
                   << " best_candidate.offset: " << Eigen::Vector3i(best_candidate.offset.x(),
                                                                    best_candidate.offset.y(),
@@ -162,27 +164,52 @@ std::vector<DiscreteScan> FastVoxelMatcher::GenerateDiscreteScans(const Rigid3f
     for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) {
         angles.push_back(rz * param.yaw_step);
     }
-    for (size_t i = 0; i != angles.size(); ++i) {
-        const Eigen::Vector3f angle_axis(0.f, 0.f, angles[i]);
-        // It's important to apply the 'angle_axis' rotation between the translation
-        // and rotation of the 'initial_pose', so that the rotation is around the
-        // origin of the range data, and yaw is in map frame.
-        const Rigid3f pose(
-            init_pose.translation(),
-            cartographer::transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
-                init_pose.rotation());
-        result.push_back(
-            CreateDiscreteScan(i, pose, point_cloud, voxel_map));
+    if ((param.mode & FAST_MODE_INIT) > 0){
+        for (size_t roll = 0; roll != angles.size(); ++roll) {
+            const Eigen::Vector3f roll_axis(angles[roll], 0.f, 0.f);
+            // It's important to apply the 'angle_axis' rotation between the translation
+            // and rotation of the 'initial_pose', so that the rotation is around the
+            // origin of the range data, and yaw is in map frame.
+            for (size_t pitch = 0; pitch != angles.size(); ++pitch) {
+                const Eigen::Vector3f pitch_axis(0.f, angles[pitch], 0.f);
+                for (size_t yaw = 0; yaw != angles.size(); ++yaw) {
+                    const Eigen::Vector3f yaw_axis(0.f, 0.f, angles[yaw]);
+                    const Rigid3f pose(
+                        init_pose.translation(),
+                        cartographer::transform::AngleAxisVectorToRotationQuaternion(
+                                    Eigen::Vector3f(roll_axis + pitch_axis + yaw_axis)) *
+                            init_pose.rotation());
+                    result.push_back(
+                        CreateDiscreteScan(roll, pitch, yaw, pose, point_cloud, voxel_map));
+                }
+
+            }
+        }
+    }else{
+        for (size_t yaw = 0; yaw != angles.size(); ++yaw) {
+            const Eigen::Vector3f yaw_axis(0.f, 0.f, angles[yaw]);
+            const Rigid3f pose(
+                init_pose.translation(),
+                cartographer::transform::AngleAxisVectorToRotationQuaternion(
+                            yaw_axis) *
+                    init_pose.rotation());
+            result.push_back(
+                CreateDiscreteScan(0, 0, yaw, pose, point_cloud, voxel_map));
+        }
     }
   return result;
 }
 
-DiscreteScan FastVoxelMatcher::CreateDiscreteScan(const int index,
+DiscreteScan FastVoxelMatcher::CreateDiscreteScan(const int roll_index,
+                                                  const int pitch_index,
+                                                  const int yaw_index,
                                                   const Rigid3f &trans_pose,
                                                   const VectorCloud &point_cloud,
                                                   const VoxelMap &voxel_map) const {
     DiscreteScan scan;
-    scan.index = index;
+    scan.roll_index = roll_index;
+    scan.pitch_index = pitch_index;
+    scan.yaw_index = yaw_index;
     scan.ground_size = 0;
     scan.above_ground_size = 0;
     scan.pose = trans_pose;
@@ -210,6 +237,7 @@ DiscreteScan FastVoxelMatcher::CreateDiscreteScan(const int index,
     return scan;
 }
 
+
 std::vector<Candidate>
 FastVoxelMatcher::GenerateLowestResolutionCandidates(const VoxelMap &voxel_map,
                                                      const std::vector<DiscreteScan> &discrete_scans,
@@ -242,7 +270,7 @@ FastVoxelMatcher::GenerateLowestResolutionCandidates(const VoxelMap &voxel_map,
     // buffer current yaw & pose
     for (int i = 0; i < candidates.size(); i++) {
         Candidate& cand = candidates[i];
-        if (cand.scan->index == discrete_scans.size() / 2)
+        if (cand.scan->yaw_index == discrete_scans.size() / 2)
             cand.score += 0.01;
         if (cand.offset[0] <= 0 && cand.offset[0] + step_size >= 0
                 && cand.offset[1] <= 0 && cand.offset[1] + step_size >= 0
@@ -255,8 +283,8 @@ FastVoxelMatcher::GenerateLowestResolutionCandidates(const VoxelMap &voxel_map,
     std::vector<Candidate> top_candidates;
     for (int i = 0; i < candidates.size(); i++) {
         const Candidate& cand = candidates[i];
-        ++ scans_count[cand.scan->index];
-        if (scans_count[cand.scan->index] <= MAX_SCAN_CANDIDATE_COUNT
+        ++ scans_count[cand.scan->yaw_index];
+        if (scans_count[cand.scan->yaw_index] <= MAX_SCAN_CANDIDATE_COUNT
             && top_candidates.size() < TOP_CANDIDATE_COUNT) {
             top_candidates.push_back(cand);
         }

libs/localize/voxel_map/fast_voxel_matcher.h

@@ -61,7 +61,10 @@ private:
         const VoxelMap &voxel_map,
         const Param &param) const;
 
-    DiscreteScan CreateDiscreteScan(const int index, const Rigid3f &trans_pose,
+    DiscreteScan CreateDiscreteScan(const int roll_index,
+                                    const int pitch_index,
+                                    const int yaw_index,
+                                    const Rigid3f &trans_pose,
                                     const VectorCloud &point_cloud,
                                     const VoxelMap &voxel_map) const;