diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp
index e9f54e30e13c545e6969dd0ad9e4d0d2ca156bdf..ae17bc94f1a4615529a9a9b865cf105a9c303ae0 100644
--- a/src/mapOptmization.cpp
+++ b/src/mapOptmization.cpp
@@ -875,6 +875,13 @@ public:
 
         downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
         downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
+        for(auto& pt : surroundingKeyPosesDS->points)
+        {
+            pointSearchInd.resize(1);
+            pointSearchSqDis.resize(1);
+            kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
+            pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
+        }
 
         // also extract some latest key frames in case the robot rotates in one position
         int numPoses = cloudKeyPoses3D->size();