Two-Dimensional-Simultaneous Localisation and Mapping Study Based on Factor Graph Elimination Optimisation

Round 1

Reviewer 1 Report

1.Detail description required for each tables and figures presented in manuscript

2.Add comparison table for proposed and existing work.

3.Required to improve image quality. Follow vector format.

4.Discuss resulted value in both abstract and conclusion

5.Add future work

Author Response

Responses to reviewer comments:

Dear Editor of Sustainability，

Dear Reviewers of Sustainability，

We thank you very much for giving us an opportunity to revise our manuscript, we appreciate editors and reviewers very much for their positive and constructive comments and suggestions on our manuscript entitled“2D-simultaneous localization and mapping study based on factor graph elimination optimization”.

We have studied reviewer’s comments carefully and have made revisions which were marked in red in the paper. We have tried our best to revise our manuscript according to the comments. Attached please find the revised version, which we would like to submit for your kind consideration.

We would like to express our great appreciation to you and reviewers for comments on our paper.

Looking forward to hearing from you.

Thank you and best regards.

Yours sincerely,

Xinzhao Wu, Peiqing Li, Qipeng Li, Zhuoran Li

Corresponding author: Peiqing Li, E-mail: [email protected]

 

Responds to the reviewer’s comments as follows:

Reviewer #1

1.Detail description required for each tables and figures presented in manuscript.

Answer: We have revised this paper. After the revision of this paper, we added more detailed descriptions of the tables and figures in the text.

2.Add comparison table for proposed and existing work.

Answer: The paper was revised to include a comparison of the recommendations and the work we have done in the conclusion section of the paper.

3.Required to improve image quality. Follow vector format.

Answer: With the modifications in this paper, we have improved the image quality and followed the corresponding format.

4.Discuss resulted value in both abstract and conclusion.

Answer: After modification of this paper, we discuss the value of the results in the summary and conclusion sections, respectively. (Please see the revised manuscript of the abstract and conclusion)

5.Add future work.

Answer: After revising this paper, we added the direction of future work at the conclusion.

Reviewer 2 Report

This paper proposed W-SDF map-based SLAM for an unmanned cart to improve recognition accuracy and reduce system memory occupation. In particular, they have proposed a smoothing-based SLAM, i.e., pose graph optimization,  that estimates the full robot trajectories using the complete set of measurements.  They have compared their proposed scheme with two traditional schemes also. The paper is well-written and the results make sense. I have the following concerns: 

1. In the first paragraph of the paper, the authors should introduce SLAM in detail with motivation, benefits, and applications by properly citing good references.  There are several applications of SLAM in mobile, AR/VR, self-driving cars, drones, video games, etc. The following reference can be used in case of vehicular localization & mapping: 

----, "Vehicle positioning based on optical camera communication in V2I environments," Computers, Materials & Continua, vol. 72, no.2, pp. 2927–2945, 2022.

2.  The authors should clearly identify the main contributions of the paper and should mention the organization of the paper. 

3. The author's work is closely related to the paper  below: 

Yang L, Ma H, Wang Y, Xia J, Wang C. "A Tightly Coupled LiDAR-Inertial SLAM for Perceptually Degraded Scenes", Sensors (Basel), vol. 22, no. 8, pp. 3063-, 15 Apr 2022;  doi: 10.3390/s22083063.

However, the above paper is not cited by the authors. This becomes an ethical issue. Please cite the above work in the introduction section and mention the novelty that the author's work got compared to the above paper. 

 

4. The authors did not explain Figure 4 in detail. Please explain. 

 

5. How the mapping will be performed after Keyframe selection? There is no information on that. That is, when a point cloud frame is detected as a keyframe, the environment map of the current frame needs to be updated. 

 

6. The authors' mentioned that "....M are fed back to the global optimization for state correction." How the state correction is performed? 

 

7. What about the calibration issues in LiDAR, IMU, and odometer? There is no information about that.

 

8. Please cite references for Hector and Cartograph algorithms that are compared in the paper. 

 

9. Figures 10, 11, 12, and 15 don't seem to provide much information or a good comparison among various algorithms. Therefore, I think it is better to provide motion trajectories, trajectory error, and the true value produced by the different methods. 

10. Moreover, in the point cloud maps provided in  Figures 10, 11, 12, and 15, the authors should include the real map so that the comparison becomes obvious and the figures become more illustrious. 

11. In all the results, the authors should clearly explain why their proposed algorithms are behaving better than the other ones. 

12. In conclusion, as the authors are talking about the speed factor of the autonomous robot, what was the speed in the current experiments? Is it uniform or accelerating? Please explain the effect of the speed factor.

 

Author Response

Responses to reviewer comments:

Dear Editor of Sustainability，

Dear Reviewers of Sustainability，

We thank you very much for giving us an opportunity to revise our manuscript, we appreciate editors and reviewers very much for their positive and constructive comments and suggestions on our manuscript entitled“2D-simultaneous localization and mapping study based on factor graph elimination optimization”.

We have studied reviewer’s comments carefully and have made revisions which were marked in red in the paper. We have tried our best to revise our manuscript according to the comments. Attached please find the revised version, which we would like to submit for your kind consideration.

We would like to express our great appreciation to you and reviewers for comments on our paper.

Looking forward to hearing from you.

Thank you and best regards.

Yours sincerely,

Xinzhao Wu, Peiqing Li, Qipeng Li, Zhuoran Li

Corresponding author: Peiqing Li, E-mail: [email protected]

 

Responds to the reviewer’s comments as follows:

Reviewer #2:

1. In the first paragraph of the paper, the authors should introduce SLAM in detail with motivation, benefits, and applications by properly citing good references.  There are several applications of SLAM in mobile, AR/VR, self-driving cars, drones, video games, etc. The following reference can be used in case of vehicular localization & mapping: 

----, "Vehicle positioning based on optical camera communication in V2I environments," Computers, Materials & Continua, vol. 72, no.2, pp. 2927–2945, 2022.

Answer: We added a total of six references, including one recommended by the reviewer, for details see Refs. [2]-[6] and [10].

2. The authors should clearly identify the main contributions of the paper and should mention the organization of the paper. 

Answer: The main innovation of this paper is to propose a tightly coupled information framework based on LiDAR, IMU and wheeled odometer data. A weighted SDF map is introduced, ground constraints and key frame selection are added to combine key frames and closed-loop detection, and finally, a factorization optimization algorithm based on factor maps is proposed. The results show that the method improves computational efficiency, positioning accuracy and error elimination. This paper is modified to include the organizational structure of this paper. (Please see the last revised paragraph of Section 1)

3. The author's work is closely related to the paper  below:

Yang L, Ma H, Wang Y, Xia J, Wang C. "A Tightly Coupled LiDAR-Inertial SLAM for Perceptually Degraded Scenes", Sensors (Basel), vol. 22, no. 8, pp. 3063-, 15 Apr 2022;  doi: 10.3390/s22083063.

However, the above paper is not cited by the authors. This becomes an ethical issue. Please cite the above work in the introduction section and mention the novelty that the author's work got compared to the above paper. 

 Answer: The innovation of this paper from the above paper is the addition of W-SDF maps in the front-end and the factor graph-based elimination optimization algorithm in the back-end, which improves the computational efficiency. After the revision of this paper, we add the research content of the above paper in the introduction section and cite it as reference.

4. The authors did not explain Figure 4 in detail. Please explain. 

 Answer: After revising this paper, we have added an explanation of the details and process of Figure 4 in Section 5.1.

5. How the mapping will be performed after Keyframe selection? There is no information on that. That is, when a point cloud frame is detected as a keyframe, the environment map of the current frame needs to be updated. 

 Answer: After the revision of this article, we have added the mapping of keyframes after selection and the method of updating keyframes to the article. (Please see the revised first paragraph of section 3.3)

6. The authors' mentioned that "....M are fed back to the global optimization for state correction." How the state correction is performed? 

 Answer: Because there is a detailed description of the state correction in reference [38], it is by solving for the corrected cost function, the current state can be used for repositioning in the global map. Therefore, the description is not expanded in this paper.

7. What about the calibration issues in LiDAR, IMU, and odometer? There is no information about that.

 Answer: The positional information under the reference system centered on the unmanned vehicle. The acceleration deviation of the IMU and the gyroscope deviation can be obtained by offline calibration. The external calibration matrix between LiDAR and IMU can be obtained by offline calibration. The initial roll and pitch of the global attitude are obtained by unbiased acceleration measurements before the motion, and the pre-integration of the IMU and the odometer is used to correct the distorted point cloud generated by the LIDAR motion.

8. Please cite references for Hector and Cartograph algorithms that are compared in the paper. 

 Answer: The paper was revised to include a reference in the second paragraph of Section 6.

9. Figures 10, 11, 12, and 15 don't seem to provide much information or a good comparison among various algorithms. Therefore, I think it is better to provide motion trajectories, trajectory error, and the true value produced by the different methods. 

Answer: In this paper, we have added some descriptions of the images. Figure 10 and Figure 11 show a comparison of the build results, and Figure 12 and Figure 15 show a comparison of the trajectories and errors, which we discuss separately.

10. Moreover, in the point cloud maps provided in Figures 10, 11, 12, and 15, the authors should include the real map so that the comparison becomes obvious and the figures become more illustrious. 

Answer: After the modifications in this paper, we added pictures of unmanned carts in real scenes, because Figure 10 and Figure 12, Figure 11 and Figure 15 are the same scene, so only real scenes are added in Figure 10 and Figure 11.

11. In all the results, the authors should clearly explain why their proposed algorithms are behaving better than the other ones. 

Answer: This paper has been modified to add a more detailed comparative description of this paper and the other two algorithms.

12. In conclusion, as the authors are talking about the speed factor of the autonomous robot, what was the speed in the current experiments? Is it uniform or accelerating? Please explain the effect of the speed factor.

Answer: In this paper, we have revised the paper to include the speed in the experiments. The speed of the unmanned vehicle is uniform during the experiments of various algorithms, because the experimental equipment is limited and needs to be experimented at low speed. (Please see the first paragraph of Section 6 of the revised paper for details)

Reviewer 3 Report

The paper '2D-simultaneous localization and mapping study based on factor graph elimination optimization' proposes a tightly-coupled 2D LiDAR based system that is more accurate and memory-efficient than existing algorithms. However, after reading this paper, I think it is badly organized and written. The technical Section of this paper is written unclearly, and I cannot fully judge the innovation of this article. The experiment section does not fully support the claim. Please find the attached suggestions. Kindly modify the content of this paper and clearly convey the technical details in future versions. 

 

1. As the authors cited, there are multiple tight-coupled 2D LiDAR SLAM. Please summarize the major contributions and innovations of this article. It seems the only contribution is the W-SDF.

 

2. All symbols should be explained. Their size and manifold should be clear.

 

3. What are T and \theta in q? Please provide the sizes of these parameters. Function M is not explained. Is it SDF? In (3), how to handle partially overlapped scans? If SDF covers the entire volume, the computation is large. Besides, the LM algorithm, Jacobian and Hessian（4-13) are routine process and can be ignored.

 

4. Please explain the strategy for the robot moving out of the pre-defined W-SDF.

 

5. In Section 3.1, the authors provide detailed descriptions of W-SDF formulation. Please comment on why it is chosen to substitute conventional 2D ICP. 

 

6. In Section 3.2, it is unclear why to constrain with a plane. I suppose q is in SE2 (although the author failed to explain), and observations are in 2D. Since ground constraint is the hard constraint, all formulations should be on the 2D manifold. Otherwise, please explain the reason and elaborate on why the 2D plane constraint is not integrated with the pose estimation procedure.

 

7. Section 4. The loop-closure algorithm is from [30] and can be simplified.  

 

8. In section 5, the authors draw a figure and list all general factor graph optimization formulations. These general factor graphs and marginalization formulations are not new and should be simplified with a citation.

 

9. In the experiment, please explain if the coding is new or based on other off-the-shelf algorithms. Since the proposed method does not differ much from conventional tightly-coupled algorithms, an ablation study is beneficial for readers.

 

10. Please provide the size and resolution of the W-SDF.

 

11. In the memory test, I am confused. The pre-defined W-SDF should occupy constant memory. But Fig. 18 shows its memory keeps increasing.

Author Response

Responses to reviewer comments:

Dear Editor of Sustainability，

Dear Reviewers of Sustainability，

We thank you very much for giving us an opportunity to revise our manuscript, we appreciate editors and reviewers very much for their positive and constructive comments and suggestions on our manuscript entitled“2D-simultaneous localization and mapping study based on factor graph elimination optimization”.

We have studied reviewer’s comments carefully and have made revisions which were marked in red in the paper. We have tried our best to revise our manuscript according to the comments. Attached please find the revised version, which we would like to submit for your kind consideration.

We would like to express our great appreciation to you and reviewers for comments on our paper.

Looking forward to hearing from you.

Thank you and best regards.

Yours sincerely,

Xinzhao Wu, Peiqing Li, Qipeng Li, Zhuoran Li

Corresponding author: Peiqing Li，E-mail: [email protected]

 

Responds to the reviewer’s comments as follows:

Reviewer #3:

1. As the authors cited, there are multiple tight-coupled 2D LiDAR SLAM. Please summarize the major contributions and innovations of this article. It seems the only contribution is the W-SDF.

 Answer: In this paper, we propose a multi-sensor fusion SLAM method based on factor graph elimination optimization for 2D-SLAM in multiple scenes. not only W-SDF is proposed, but also factor graph based elimination optimization algorithm, which contributes in reducing algorithm memory, improving localization accuracy and enhancing computational efficiency. This time, the abstract and conclusion sections have been revised to make the contributions and innovations clearer.

2. All symbols should be explained. Their size and manifold should be clear.

 Answer: This article has been revised to include a more detailed explanation of the symbols.

3. What are T and \theta in q? Please provide the sizes of these parameters. Function M is not explained. Is it SDF? In (3), how to handle partially overlapped scans? If SDF covers the entire volume, the computation is large. Besides, the LM algorithm, Jacobian and Hessian（4-13) are routine process and can be ignored.

 Answer: After modification in this paper, T is the relative translation between the robot's current pose and the previous moment pose, and  is the relative rotation angle between the robot's current pose and the previous moment pose. In order to solve for the relative pose of the robot between the two moments, the rotation transformation of the current scan point is usually performed using the previous moment pose, and then projected onto the existing map M. In the case of an obstacle in the raster, the contour line of the obstacle can be calculated according to the method of equation (4 ) ~ (7) method to calculate the intersection of the obstacle and the two edges in this raster, so as to obtain the contour line of the obstacle, and then the line can be drawn as the contour of the object in this raster, so that the map has finer map accuracy compared with occupying the raster map directly the whole raster as the contour of the object. The LM algorithm, Jacobian and Hessian make reservations for the sake of article completeness and readability.

4. Please explain the strategy for the robot moving out of the pre-defined W-SDF.

 Answer: This paper has been modified to explain that when stepping out of predefined or when there are not enough available scan points in a raster, the regression should be done with the scan points located in the adjacent raster.

5. In Section 3.1, the authors provide detailed descriptions of W-SDF formulation. Please comment on why it is chosen to substitute conventional 2D ICP. 

 Answer: Since the main drawback of most ICP-based algorithms is the high complexity of searching for corresponding points during each iteration, Hector SLAM uses a scheme that matches the current scan endpoint with the map value, which does not require searching for corresponding points. Therefore, in this paper, we introduce weighted SDF maps to raster maps based on Hector SLAM and improve the Gaussian Newton method for solving the scan matching problem in Hector SLAM using the L-M method.

6. In Section 3.2, it is unclear why to constrain with a plane. I suppose q is in SE2 (although the author failed to explain), and observations are in 2D. Since ground constraint is the hard constraint, all formulations should be on the 2D manifold. Otherwise, please explain the reason and elaborate on why the 2D plane constraint is not integrated with the pose estimation procedure.

Answer: Due to the unevenness of the ground over which the robot passes, it is not possible to construct pose maps with fixed planes as nodes for constraints. In order to ensure that the constructed constraints are consistent with the actual situation, a subgraph-based ground extraction method is proposed.

7. Section 4. The loop-closure algorithm is from [30] and can be simplified.  

 Answer: This paper has been modified so that if the closed-loop detection algorithm continues to be simplified, it may not reflect the integrity of closed-loop detection.

8. In section 5, the authors draw a figure and list all general factor graph optimization formulations. These general factor graphs and marginalization formulations are not new and should be simplified with a citation.

 Answer: This paper has been revised to include cited references [41].

9. In the experiment, please explain if the coding is new or based on other off-the-shelf algorithms. Since the proposed method does not differ much from conventional tightly-coupled algorithms, an ablation study is beneficial for readers.

 Answer: The overall framework of the SLAM algorithm proposed in this paper is part of our innovation, which we explain in the paper, and the Hector and Cartographer algorithms belong to the cited ones.

10. Please provide the size and resolution of the W-SDF.

 Answer: The W-SDF map is an improved map based on the occupied raster map. The map resolution (grid size) for the experiments in this paper is 0.05 m. The above has also been added to the first paragraph of Section 6 of the paper.

11. In the memory test, I am confused. The pre-defined W-SDF should occupy constant memory. But Fig. 18 shows its memory keeps increasing.

Answer: The memory of the two algorithms compared in this paper, Hector and Cartographer, is increasing all the time, but the initial stage of the algorithm proposed in this paper and the environment changes more will cause some fluctuations, which are normal fluctuations in the changes.

 

Round 2

Reviewer 2 Report

The authors have addressed my comments satisfactorily. I have no further comments.

Author Response

Responses to reviewer comments:

Dear Editor of Sustainability，

Dear Reviewers of Sustainability，

We appreciate again editors and reviewers very much for their positive and constructive comments and suggestions on our manuscript entitled “2D-simultaneous localization and mapping study based on factor graph elimination optimization”.

We have studied reviewer’s comments carefully and have made revisions which were marked in red in the paper. We have tried our best to revise our manuscript according to the comments. Attached please find the revised version, which we would like to submit for your kind consideration.

We would like to express our great appreciation to you and reviewers for comments on our paper.

Looking forward to hearing from you.

Thank you and best regards.

Yours sincerely,

Xinzhao Wu, Peiqing Li, Qipeng Li and Zhuoran Li

Corresponding author: Peiqing Li,

E-mail: [email protected]; Tel.: +86-0571-8507-0214

Reviewer 3 Report

1. I still think summarizing the innovations is beneficial to readers. It is difficult to distinguish the proposed paper from previous works.

 

2. In Eq. (1), the squared bracket is not necessary. How can W_{max} control the maximum weight as it is used as a coefficient? What is function R() in Eq.(2)? Is M() discrete or continuous?

 

3. A full formulation of the factor graph is helpful, including the pose graph, IMU, odometry, and plane constraint. Eq. 22-24 are only the general form of the factor graph.

 

4. Do the authors indicate that the robot should always remain in the pre-defined raster? It should be noticed that in practice there is not enough space to have a fine-resolution pre-defined map for indoor localization. Overall size of the raster map and memory consumption are very helpful to readers.

 

5. It is widely agreed that ICP is more accurate than SDF-based matching. Considering the small size of 2D points, searching with FLANN algorithm is not slow. Although the authors do not cite any research to support their motivation, their experiments do show SDF is more accurate. I have to admit that the experiment is against my previous experience. My other concern is why it is more memory efficient. Hector and Cartographer store the map in the form of 2D points.

 

Author Response

Responses to reviewer comments:

Dear Editor of Sustainability，

Dear Reviewers of Sustainability，

We appreciate again editors and reviewers very much for their positive and constructive comments and suggestions on our manuscript entitled “2D-simultaneous localization and mapping study based on factor graph elimination optimization”.

We have studied reviewer’s comments carefully and have made revisions which were marked in red in the paper. We have tried our best to revise our manuscript according to the comments. Attached please find the revised version, which we would like to submit for your kind consideration.

We would like to express our great appreciation to you and reviewers for comments on our paper.

Looking forward to hearing from you.

Thank you and best regards.

Yours sincerely,

Xinzhao Wu, Peiqing Li, Qipeng Li and Zhuoran Li

Corresponding author: Peiqing Li,

E-mail: [email protected]; Tel.: +86-0571-8507-0214

 

Responds to the reviewer’s comments as follows:

Reviewer #3

 

1. I still think summarizing the innovations is beneficial to readers. It is difficult to distinguish the proposed paper from previous works.

 

Answer: The paper has been revised and we have provided a more detailed summary of the innovative summary and conclusion sections.

 

2. In Eq. (1), the squared bracket is not necessary. How can W_{max} control the maximum weight as it is used as a coefficient? What is function R() in Eq.(2)? Is M() discrete or continuous?

 

Answer: With the modifications in this paper, we have removed the square brackets from equation (1). The  used in this paper is a fixed value. R() does not appear in equation (2). Is the R() you mention involved in equation (3) ,which is a rotation matrix, modified to be included in this paper. M() in our text is continuous.

 

3. A full formulation of the factor graph is helpful, including the pose graph, IMU, odometry, and plane constraint. Eq. 22-24 are only the general form of the factor graph.

 

Answer: The full formula for the factor graph is useful in this paper in the cited literature [39], so only a brief description of the factor graph is given in this paper to reduce redundancy for ease of reading.

 

4. Do the authors indicate that the robot should always remain in the pre-defined raster? It should be noticed that in practice there is not enough space to have a fine-resolution pre-defined map for indoor localization. Overall size of the raster map and memory consumption are very helpful to readers.

 

Answer: The robots in this paper will always remain in the predefined raster. With modifications, we have increased the overall size of the raster map in section 6.

 

5. It is widely agreed that ICP is more accurate than SDF-based matching. Considering the small size of 2D points, searching with FLANN algorithm is not slow. Although the authors do not cite any research to support their motivation, their experiments do show SDF is more accurate. I have to admit that the experiment is against my previous experience. My other concern is why it is more memory efficient. Hector and Cartographer store the map in the form of 2D points.

 

Answer: The experimental equipment and the real environment of the experiments conducted in this paper may differ from previous experiments. Our proposed factor graph elimination algorithm adds a sliding window to the chained factor graph model in order to improve fault tolerance and retain the historical state information within the window; at the same time, in order to avoid high-dimensional matrix operations, the elimination algorithm is introduced to transform the factor graph into a Bayesian network, which sequentially marginalises the historical states to achieve matrix dimensionality reduction. In the comparison experiments, the proposed factor graph elimination optimisation algorithm can significantly improve the accuracy and reliability of localisation, while greatly reducing the amount of information fusion operations and memory occupation. For ease of understanding, we have modified and added the above explanation in the conclusion section (3) of the paper.

 

Author Response File: Author Response.pdf

Round 3

Reviewer 3 Report

I do not have further questions.