Trying to fill holes in occupancy grid uting OpenCV - c++

Question

I have this project where we are trying to make an autonomous vehicle using a lidar and a stereo camera. To to this we're making two maps with cartographer and merging them together. However, the data from the stereo camera is not very accurate and we therefor have to manipulate the map made by cartographer. To make the camera map we are detecting lines, reading the distance and turning this into a laser scan which is the sent to cartographer. Ideally we would be able to convert the map into just the lines. This is what the camera map looks like: Camera map
What I would like to do first is fill out the holes in the map to make it easier to find lines and such later. This is where I am struggling. I have written code to convert from nav_msgs::OccupancyGrid to cv::Mat and back in addition to merging the maps. I have looked over this code and I don't think this is where the problem is. I have tried different suggestions online but have not gotten close to a solution. This is my code:
cv::Mat fill_cam_mat(cv::Mat mat) {
int thresh = 50;
cv::Mat canny_output;
cv::Canny( mat, canny_output, thresh, thresh*2 );
//std::vector<cv::Vec4i> hierarchy;
cv::Mat mat_floodfill = canny_output.clone();
cv::floodFill(mat_floodfill, cv::Point(0,0), cv::Scalar(255));
cv::Mat mat_floodfill_inv;
cv::bitwise_not(mat_floodfill, mat_floodfill_inv);
cv::Mat mat_out = (canny_output | mat_floodfill_inv);
return mat_out;
}
And my result is as follows when merged with the lidar map:
Final map
I have also tried:
cv::Mat fill_cam_mat(cv::Mat mat) {
int mat_height = mat.rows;
int mat_width = mat.cols;
int thresh = 50;
cv::Mat canny_output;
cv::Canny( mat, canny_output, thresh, thresh*2 );
cv::Mat non_zero;
cv::findNonZero(canny_output, non_zero);
std::vector<std::vector<cv::Point>> hull(non_zero.total());
for(unsigned int i = 0, n = non_zero.total(); i < n; ++i) {
cv::convexHull(non_zero, hull[i], false);
}
cv::Mat fill_contours_result(mat_height, mat_width, CV_8UC3, cv::Scalar(0));
cv::fillPoly(fill_contours_result, hull, 255);
return fill_contours_result;
}
Which gives the same result. I have also tried using cv::findContours to spicify the hull, but that worked even worse.
I am new with OpenCV and I don't understand what is wrong with my output. Would really appreciate any help on the code or if anybody have any better suggestions on how to solve the problem. Is it even necessary to fill the holes in order to get useful information from the map?
Thank you in advance!