cv_test, icp_process, pointCloudToLDP, laserScanToLDP add angle filter

CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

ceres_test/src/ceres_test.cpp

@@ -10,6 +10,7 @@ struct CURVE_FITTING_COST
 {
   CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
   template <typename T>
+  //使用仿函数重载
   bool operator()(const T* const abc,T* residual)const
   {
     residual[0]=_y-ceres::exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);
@@ -54,7 +55,9 @@ int main()
 //配置求解器并求解，输出结果
   ceres::Solver::Options options;
   options.linear_solver_type=ceres::DENSE_QR;
-  options.minimizer_progress_to_stdout=true;
+  options.minimizer_progress_to_stdout = true;
+  options.parameter_tolerance = 1e-9;
+  options.function_tolerance = 1e-9;
   ceres::Solver::Summary summary;
   ceres::Solve(options,&problem,&summary);
   cout<<"a= "<<abc[0]<<endl;

cpp_pkgs/src/basicTest.cpp

@@ -5,6 +5,8 @@
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
+#include <math.h>
+
 #define trace(x, format) \
 printf(#x " = %" #format "\n", x)
 #define trace2(i) trace(x ## i, d)
@@ -76,7 +78,7 @@ int main(){
     int x = 66, x1=12;
     trace(x, c);
     trace2(1);
-    
+    std::cout<<int(-2.5)<<std::endl;
 }
 
 #endif

cv_test/CMakeLists.txt

@@ -0,0 +1,198 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(cv_test)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+)
+find_package(OpenCV REQUIRED)
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES cv_test
+#  CATKIN_DEPENDS roscpp
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INLCUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/cv_test.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/cv_core.cpp)
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS})
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_cv_test.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

cv_test/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cv_test</name>
+  <version>0.0.0</version>
+  <description>The cv_test package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/cv_test</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

cv_test/src/cv_core.cpp

@@ -0,0 +1,34 @@
+#include <stdio.h>
+#include <opencv2/opencv.hpp>
+
+int main(int argc, char** argv )
+{
+    char * addr = "./src/cv_test/backGround.jpg";
+    cv::String imageName(addr);
+    //创建Mat
+    cv::Mat image;
+    //读取图片
+    image = cv::imread(imageName, 1 );
+    //判断图片是否存在
+    if(!image.empty() && image.data){
+        //创建窗口
+        cv::namedWindow("Display Image", cv::WINDOW_AUTOSIZE );
+        //显示图片
+        cv::imshow("Display Image", image);
+        //彩图转灰度图
+        cv::Mat gray_image;
+        cv::cvtColor(image, gray_image, cv::COLOR_BGR2GRAY);
+        cv::namedWindow("Display Image2", cv::WINDOW_AUTOSIZE );
+        cv::imshow("Display Image2", gray_image);
+        //保存图片
+        cv::imwrite( "./src/cv_test/Gray_Image.jpg", gray_image);
+        //中断
+        cv::waitKey(0);
+    }else
+    {
+        std::cout<<imageName<<std::endl;
+        std::cout<<"empty image"<<std::endl;
+    }
+    
+    return 0;
+}

scan_matcher_ICP/src/laser_scan_matcher.cpp

@@ -35,7 +35,7 @@
  *  on Robotics and Automation (ICRA), 2008
  */
 
-#include <laser_scan_matcher.h>
+#include "laser_scan_matcher.h"
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/common/transforms.h>
 #include <pcl/registration/icp.h>
@@ -193,7 +193,7 @@ void LaserScanMatcher::initParams()
   //  1) Order the errors.
   //  2) Choose the percentile according to outliers_adaptive_order.
   //     (if it is 0.7, get the 70% percentile)
-  //  3) Define an adaptive threshold multiplying outliers_adaptive_mult
+  //  3) Define an adaptive threshold multiplying outliers_adaptive_multinput_
   //     with the value of the error at the chosen percentile.
   //  4) Discard correspondences over the threshold.
   //  This is useful to be conservative; yet remove the biggest errors.
@@ -249,7 +249,7 @@ void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& sca
       return;
     }
 
-    laserScanToLDP(scan_msg, prev_ldp_scan_, scan_msg->ranges.size());
+    laserScanToLDP(scan_msg, prev_ldp_scan_, scan_msg->angle_min, scan_msg->angle_max);
     last_icp_time_ = scan_msg->header.stamp;
     initialized_ = true;
 
@@ -286,20 +286,19 @@ void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& sca
 
     //printf("min_ang, max_ang, ang_inc: %.3f %.3f %.3f\n",scan_msg->angle_min, scan_msg->angle_max, scan_msg->angle_increment);
   }
-  LDP curr_ldp_scan;
-  laserScanToLDP(scan_msg, curr_ldp_scan, scan_msg->ranges.size()/2);
+  laserScanToLDP(scan_msg, curr_ldp_scan_, -M_PI_2, M_PI_2);
  
   //for cloud display
-  pc_curr_.width = curr_ldp_scan->nrays;
+  pc_curr_.width = curr_ldp_scan_->nrays;
   pc_curr_.height = 1;
-  pc_curr_.resize(curr_ldp_scan->nrays);
-  for (int i = 0; i < curr_ldp_scan->nrays; i++)
+  pc_curr_.resize(curr_ldp_scan_->nrays);
+  for (int i = 0; i < curr_ldp_scan_->nrays; i++)
   {
-    // printf("curr i, valid[i]: %d, %d\n",i,curr_ldp_scan->valid[i]);
-    if (curr_ldp_scan->valid[i])
+    // printf("curr i, valid[i]: %d, %d\n",i,curr_ldp_scan_->valid[i]);
+    if (curr_ldp_scan_->valid[i])
     {
-      dist = curr_ldp_scan->readings[i];
-      theta = curr_ldp_scan->theta[i];
+      dist = curr_ldp_scan_->readings[i];
+      theta = curr_ldp_scan_->theta[i];
       pc_curr_.points[i].x = dist * cos(theta);
       pc_curr_.points[i].y = dist * sin(theta);
       pc_curr_.points[i].z = 0;
@@ -312,12 +311,70 @@ void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& sca
     }
     // printf("curr x,y,z: %.3f, %.3f, %.3f\n", pc_curr_.points[i].x, pc_curr_.points[i].y, pc_curr_.points[i].z);
   }
-  processScan(curr_ldp_scan, scan_msg->header.stamp);
+  // tf::Transform t;
+  // icp_process(pc_origin_, pc_curr_, t);
+  processScan(curr_ldp_scan_, scan_msg->header.stamp);
+  
 }
+void LaserScanMatcher::icp_process(const pcl::PointCloud<pcl::PointXYZ> frame_left, const pcl::PointCloud<pcl::PointXYZ> frame_right, tf::Transform transform)
+{
+  ros::WallTime start = ros::WallTime::now();
+  LDP prev, curr;
+  pointCloudToLDP(frame_left.makeShared(), prev, -M_PI, M_PI);
+  pointCloudToLDP(frame_right.makeShared(), curr, -M_PI/2.0, M_PI/2.0);
+  prev->odometry[0] = 0.0;
+  prev->odometry[1] = 0.0;
+  prev->odometry[2] = 0.0;
+
+  prev->estimate[0] = 0.0;
+  prev->estimate[1] = 0.0;
+  prev->estimate[2] = 0.0;
+
+  prev->true_pose[0] = 0.0;
+  prev->true_pose[1] = 0.0;
+  prev->true_pose[2] = 0.0;
+
+  input_.laser_ref  = prev;
+  input_.laser_sens = curr;
+
+  input_.first_guess[0] = f2b_.getOrigin().getX();
+  input_.first_guess[1] = f2b_.getOrigin().getY();
+  input_.first_guess[2] = tf::getYaw(f2b_.getRotation());
+
+  // If they are non-Null, free covariance gsl matrices to avoid leaking memory
+  //清空GNU科学计算库协方差矩阵
+  if (output_.cov_x_m)
+  {
+    gsl_matrix_free(output_.cov_x_m);
+    output_.cov_x_m = 0;
+  }
+  if (output_.dx_dy1_m)
+  {
+    gsl_matrix_free(output_.dx_dy1_m);
+    output_.dx_dy1_m = 0;
+  }
+  if (output_.dx_dy2_m)
+  {
+    gsl_matrix_free(output_.dx_dy2_m);
+    output_.dx_dy2_m = 0;
+  }
+
+  ros::WallTime end0 = ros::WallTime::now();
+  // ******************** csm ICP配准 ********************
+  sm_icp(&input_, &output_);
 
+  ros::WallTime end1 = ros::WallTime::now();
+  double dur1 = (end1 - end0).toSec() * 1e3;
+  ROS_INFO("Scan matcher total duration: %.2f ms", dur1);
+  printf("icp_process transform x,y,theta: %.3f, %.3f, %.3f\n",output_.x[0],output_.x[1],output_.x[2]);
+  transform.setOrigin(tf::Vector3(output_.x[0], output_.x[1], 0.0));
+  tf::Quaternion q;
+  q.setRPY(0.0, 0.0, output_.x[2]);
+  transform.setRotation(q);
+}
 void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
 {
-  // 发布tf模板
+  // // 发布tf模板
   // tf::TransformBroadcaster tf_broadcaster_;
   // tf::Transform transform;
   // transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0));
@@ -432,7 +489,7 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
   //Setting Resolution of NDT grid structure (VoxelGridCovariance).
   ndt.setResolution (0.5);
   // Setting max number of registration iterations.
-  ndt.setMaximumIterations (60);
+  ndt.setMaximumIterations (50);
   // Setting point cloud to be aligned.
   ndt.setInputSource (cur_ptr);
   // Setting point cloud to be aligned to.
@@ -442,10 +499,10 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
   ndt_omp->setInputTarget(ori_ptr);
   ndt_omp->setInputSource(cur_ptr);
   ndt_omp->setNumThreads(n);
-  ndt_omp->setTransformationEpsilon(0.0001);
-  ndt_omp->setStepSize(0.05);
+  ndt_omp->setTransformationEpsilon(0.00001);
+  ndt_omp->setStepSize(0.1);
   ndt_omp->setResolution(0.5);
-  ndt_omp->setMaximumIterations(35);
+  ndt_omp->setMaximumIterations(40);
   ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
 
   // Set initial alignment estimate found using robot odometry.
@@ -467,7 +524,7 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
 
   Eigen::Matrix4f pcl_trans= ndt_omp->getFinalTransformation();
 	//输出最终的变换矩阵（4x4）
-	printf("pcl transform x,y,theta: %.3f, %.3f, %.3f\n",pcl_trans(0,3),pcl_trans(1,3),atan2(-pcl_trans(0,1), pcl_trans(0,0)));
+	// printf("pcl transform x,y,theta: %.3f, %.3f, %.3f\n",pcl_trans(0,3),pcl_trans(1,3),atan2(-pcl_trans(0,1), pcl_trans(0,0)));
   // std::cout << pcl_trans << std::endl;
   ros::WallTime end2 = ros::WallTime::now();
   double dur2 = (end2 - end1).toSec() * 1e3;
@@ -561,26 +618,26 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
 
   // **** swap old and new
 
-  if (newKeyframeNeeded(corr_ch))
-  {
-    // generate a keyframe
-    ld_free(prev_ldp_scan_);
-    prev_ldp_scan_ = curr_ldp_scan;
-    f2b_kf_ = f2b_;
-    std::cout<<"new key generated.............."<<std::endl;
-  }
-  else
-  {
+  // if (newKeyframeNeeded(corr_ch))
+  // {
+  //   // generate a keyframe
+  //   ld_free(prev_ldp_scan_);
+  //   prev_ldp_scan_ = curr_ldp_scan;
+  //   f2b_kf_ = f2b_;
+  //   std::cout<<"new key generated.............."<<std::endl;
+  // }
+  // else
+  // {
     ld_free(curr_ldp_scan);
     // ld_free(ldp_prev);
-  }
+  // }
 
   last_icp_time_ = time;
 
   // **** statistics
   ros::WallTime end3 = ros::WallTime::now();
   double dur3 = (end3 - start).toSec() * 1e3;
-  std::cout<<"time consumption(csm, p1 p2 pcl and total): "<<dur1<<", "/* <<dur1_1<<", "<<dur1_2<<", "*/<<dur2<<", "<<dur3<<std::endl;
+  // std::cout<<"time consumption(csm, p1 p2 pcl and total): "<<dur1<<", "/* <<dur1_1<<", "<<dur1_2<<", "*/<<dur2<<", "<<dur3<<std::endl;
 }
 
 bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d)
@@ -595,39 +652,154 @@ bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d)
 }
 
 void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
-                                            LDP& ldp, unsigned int count)
+                                            LDP& ldp, double angle_min, double angle_max)
 {
   unsigned int n = scan_msg->ranges.size();
-  if(count>n || count<0)
+  int left=0,right=0;
+  if(angle_min>=-M_PI-1e04 && angle_min<=M_PI*2+1e04 && angle_max>=-M_PI-1e04 && angle_max<=M_PI*2+1e04 && angle_min<=angle_max){
+    if (angle_min >= scan_msg->angle_min && angle_max <= scan_msg->angle_max)
+    {
+      left = int((angle_min - scan_msg->angle_min) / scan_msg->angle_increment);
+      right = int((angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+      n = right - left + 1;
+    }else if((angle_min+M_PI*2) <= scan_msg->angle_max){
+      left = int((angle_min+M_PI*2 - scan_msg->angle_min) / scan_msg->angle_increment);
+      right = int((angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+      n = right + scan_msg->ranges.size()-left;
+    }else{
+      std::cout<<"angle limit"<<std::endl;
+      ldp = ld_alloc_new(0);
+      return;
+    }
+  }
+  else
+  {
+    std::cout<<"angle not included."<<angle_min<<","<<angle_max<<std::endl;
+    ldp = ld_alloc_new(0);
     return;
+  }
+  // if(count>n || count<0)
+  //   return;
   ldp = ld_alloc_new(n);
+  int index = 0;
   //printf("n, ang_lim: %d, %.3f\n",n,scan_msg->angle_min + n * scan_msg->angle_increment);
   for (unsigned int i = 0; i < n; i++)
   {
+    if (left <= right)
+    {
+      index = left + i;
+    }
+    else if (left > right)
+    {
+      if (i <= right)
+        index = i;
+      else
+        index = left + i - right;
+    }
+    else
+    {
+      return;
+    }
     // calculate position in laser frame
-
-    double r = scan_msg->ranges[i];
-
-    if (r > scan_msg->range_min && r < scan_msg->range_max && i<=count)
+    double r = scan_msg->ranges[index];
+    if (r > scan_msg->range_min && r < scan_msg->range_max)
     {
       // fill in laser scan data
-
       ldp->valid[i] = 1;
       ldp->readings[i] = r;
     }
     else
     {
       ldp->valid[i] = 0;
-      ldp->readings[i] = -1;  // for invalid range -1
+      ldp->readings[i] = -1; // for invalid range -1
     }
+    ldp->theta[i] = scan_msg->angle_min + index * scan_msg->angle_increment;
+    ldp->cluster[i] = -1;
+  }
 
-    ldp->theta[i]    = scan_msg->angle_min + i * scan_msg->angle_increment;
+  ldp->min_theta = ldp->theta[0];
+  ldp->max_theta = ldp->theta[n - 1];
 
-    ldp->cluster[i]  = -1;
+  ldp->odometry[0] = 0.0;
+  ldp->odometry[1] = 0.0;
+  ldp->odometry[2] = 0.0;
+
+  ldp->true_pose[0] = 0.0;
+  ldp->true_pose[1] = 0.0;
+  ldp->true_pose[2] = 0.0;
+}
+void LaserScanMatcher::pointCloudToLDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr &point_cloud_msg,
+                     LDP &ldp, double angle_min, double angle_max)
+{
+  pcl::PointCloud<pcl::PointXYZ> laserCloudIn = *point_cloud_msg;
+  // pcl::fromROSMsg(*point_cloud_msg, laserCloudIn);
+  // std::vector<int> indices;
+  // pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
+  double theta_min = atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
+  unsigned int n = laserCloudIn.points.size();
+  double theta_max = atan2(laserCloudIn.points[n-1].y, laserCloudIn.points[n-1].x);
+  std::cout<<laserCloudIn.points[0].x<<", "<<laserCloudIn.points[0].y<<std::endl;
+  std::cout<<laserCloudIn.points[n-1].x<<", "<<laserCloudIn.points[n-1].y<<std::endl;
+  if (theta_max <= 1e-4)
+    theta_max += M_PI * 2;
+  double angle_increment = (theta_max - theta_min) / n;
+  int left=0,right=0;
+
+  if(angle_min>=-M_PI-1e04 && angle_min<=M_PI*2+1e04 && angle_max>=-M_PI-1e04 && angle_max<=M_PI*2+1e04 && angle_min<=angle_max){
+    if (angle_min >= theta_min && angle_max <= theta_max)
+    {
+      left = int((angle_min - theta_min) / angle_increment);
+      right = int((angle_max - theta_min) / angle_increment);
+      n = right - left + 1;
+    }else if((angle_min+M_PI*2) <= theta_max){
+      left = int((angle_min+M_PI*2 - theta_min) / angle_increment);
+      right = int((angle_max - theta_min) / angle_increment);
+      n = right + n-left;
+    }else{
+      printf("a min max, theta min max: %.3f, %.3f, %.3f, %.3f",angle_min, angle_max, theta_min, theta_max);
+      std::cout<<"cloud angle limit"<<std::endl;
+      ldp = ld_alloc_new(0);
+      return;
+    }
+  }
+  else
+  {
+    std::cout<<"cloud angle not included."<<angle_min<<","<<angle_max<<std::endl;
+    ldp = ld_alloc_new(0);
+    return;
+  }
+  ldp = ld_alloc_new(n);
+  int index = 0;
+  //printf("n, ang_lim: %d, %.3f\n",n,scan_msg->angle_min + n * scan_msg->angle_increment);
+  for (unsigned int i = 0; i < n; i++)
+  {
+    if (left <= right)
+    {
+      index = left + i;
+    }
+    else if (left > right)
+    {
+      if (i <= right)
+        index = i;
+      else
+        index = left + i - right;
+    }
+    else
+    {
+      ldp = ld_alloc_new(0);
+      return;
+    }
+    // calculate position in laser frame
+    double r = sqrt(pow(laserCloudIn.points[index].x,2)+pow(laserCloudIn.points[index].y,2));
+    // fill in laser scan data
+    ldp->valid[i] = 1;
+    ldp->readings[i] = r;
+    ldp->theta[i] = theta_min + index * angle_increment;
+    ldp->cluster[i] = -1;
   }
 
   ldp->min_theta = ldp->theta[0];
-  ldp->max_theta = ldp->theta[n-1];
+  ldp->max_theta = ldp->theta[n - 1];
 
   ldp->odometry[0] = 0.0;
   ldp->odometry[1] = 0.0;
@@ -637,7 +809,6 @@ void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& sc
   ldp->true_pose[1] = 0.0;
   ldp->true_pose[2] = 0.0;
 }
-
 void LaserScanMatcher::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
 {
   a_cos_.clear();

scan_matcher_ICP/src/laser_scan_matcher.h

@@ -120,6 +120,7 @@ class LaserScanMatcher
     sm_params input_;
     sm_result output_;
     LDP prev_ldp_scan_;
+    LDP curr_ldp_scan_;
 
     ros::Publisher  origin_cloud_publisher_;
     ros::Publisher  current_cloud_publisher_;
@@ -136,7 +137,10 @@ class LaserScanMatcher
     void processScan(LDP& curr_ldp_scan, const ros::Time& time);
 
     void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
-                              LDP& ldp, unsigned int count);
+                              LDP& ldp, double angle_min, double angle_max);
+
+    void pointCloudToLDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr &point_cloud_msg,
+                        LDP &ldp, double angle_min, double angle_max);
 
     void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
 
@@ -147,6 +151,10 @@ class LaserScanMatcher
     bool newKeyframeNeeded(const tf::Transform& d);
 
     void createTfFromXYTheta(double x, double y, double theta, tf::Transform& t);
+
+    void icp_process(pcl::PointCloud<pcl::PointXYZ> frame_left, pcl::PointCloud<pcl::PointXYZ> frame_right, tf::Transform transform);
+
+    void ndt_process(pcl::PointCloud<pcl::PointXYZ> frame_left, pcl::PointCloud<pcl::PointXYZ> frame_right, tf::Transform transform);
 };
 
 } // namespace scan_tools