Fix 'Invalid (NaN, Inf) point coordinates given to nearestKSearch\!' …

…error
laboshinl · May 11, 2016 · 60db8c1 · 60db8c1
1 parent affb43c
commit 60db8c1
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 2 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -10,14 +10,14 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   tf)
 
-find_package(Eigen3 REQUIRED)
+#find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED)
 find_package(OpenCV REQUIRED)
 
 include_directories(
   include
 	${catkin_INCLUDE_DIRS} 
-	${EIGEN3_INCLUDE_DIR} 
+#	${EIGEN3_INCLUDE_DIR}
 	${PCL_INCLUDE_DIRS})
 
 catkin_package(

diff --git a/src/laserOdometry.cpp b/src/laserOdometry.cpp
@@ -800,6 +800,10 @@ int main(int argc, char** argv)
           transform[4] += matX.at<float>(4, 0);
           transform[5] += matX.at<float>(5, 0);
 
+          for(int i=0; i<6; i++){
+            if(isnan(transform[i]))
+              transform[i]=0;
+          }
           float deltaR = sqrt(
                               pow(rad2deg(matX.at<float>(0, 0)), 2) +
                               pow(rad2deg(matX.at<float>(1, 0)), 2) +
@@ -843,6 +847,9 @@ int main(int argc, char** argv)
       transformSum[4] = ty;
       transformSum[5] = tz;
 
+//      for(int i=0; i<6; i++){
+//        std::cout << "222" << transform[0]<<std::endl;
+//      }
       geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);
 
       laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);