├── README.md ├── calib ├── 001665.txt ├── 002893.txt └── 003969.txt ├── image_2 ├── 001665.png ├── 002893.png └── 003969.png ├── main.cpp └── velodyne ├── 001665.bin ├── 002893.bin └── 003969.bin /README.md: -------------------------------------------------------------------------------- 1 | # depth_fusion 2 | Implementation of Autocalibration of lidar and optical cameras via edge alignment by Juan Castorena et al. 3 | -------------------------------------------------------------------------------- /calib/001665.txt: -------------------------------------------------------------------------------- 1 | P0: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 0.000000000000e+00 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 2 | P1: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.875744000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 3 | P2: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 4.485728000000e+01 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.163791000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.745884000000e-03 4 | P3: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.395242000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.199936000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.729905000000e-03 5 | R0_rect: 9.999239000000e-01 9.837760000000e-03 -7.445048000000e-03 -9.869795000000e-03 9.999421000000e-01 -4.278459000000e-03 7.402527000000e-03 4.351614000000e-03 9.999631000000e-01 6 | Tr_velo_to_cam: 7.533745000000e-03 -9.999714000000e-01 -6.166020000000e-04 -4.069766000000e-03 1.480249000000e-02 7.280733000000e-04 -9.998902000000e-01 -7.631618000000e-02 9.998621000000e-01 7.523790000000e-03 1.480755000000e-02 -2.717806000000e-01 7 | Tr_imu_to_velo: 9.999976000000e-01 7.553071000000e-04 -2.035826000000e-03 -8.086759000000e-01 -7.854027000000e-04 9.998898000000e-01 -1.482298000000e-02 3.195559000000e-01 2.024406000000e-03 1.482454000000e-02 9.998881000000e-01 -7.997231000000e-01 8 | 9 | -------------------------------------------------------------------------------- /calib/002893.txt: -------------------------------------------------------------------------------- 1 | P0: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 0.000000000000e+00 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 2 | P1: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.875744000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 3 | P2: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 4.485728000000e+01 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.163791000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.745884000000e-03 4 | P3: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.395242000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.199936000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.729905000000e-03 5 | R0_rect: 9.999239000000e-01 9.837760000000e-03 -7.445048000000e-03 -9.869795000000e-03 9.999421000000e-01 -4.278459000000e-03 7.402527000000e-03 4.351614000000e-03 9.999631000000e-01 6 | Tr_velo_to_cam: 7.533745000000e-03 -9.999714000000e-01 -6.166020000000e-04 -4.069766000000e-03 1.480249000000e-02 7.280733000000e-04 -9.998902000000e-01 -7.631618000000e-02 9.998621000000e-01 7.523790000000e-03 1.480755000000e-02 -2.717806000000e-01 7 | Tr_imu_to_velo: 9.999976000000e-01 7.553071000000e-04 -2.035826000000e-03 -8.086759000000e-01 -7.854027000000e-04 9.998898000000e-01 -1.482298000000e-02 3.195559000000e-01 2.024406000000e-03 1.482454000000e-02 9.998881000000e-01 -7.997231000000e-01 8 | 9 | -------------------------------------------------------------------------------- /calib/003969.txt: -------------------------------------------------------------------------------- 1 | P0: 7.070493000000e+02 0.000000000000e+00 6.040814000000e+02 0.000000000000e+00 0.000000000000e+00 7.070493000000e+02 1.805066000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 2 | P1: 7.070493000000e+02 0.000000000000e+00 6.040814000000e+02 -3.797842000000e+02 0.000000000000e+00 7.070493000000e+02 1.805066000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 3 | P2: 7.070493000000e+02 0.000000000000e+00 6.040814000000e+02 4.575831000000e+01 0.000000000000e+00 7.070493000000e+02 1.805066000000e+02 -3.454157000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 4.981016000000e-03 4 | P3: 7.070493000000e+02 0.000000000000e+00 6.040814000000e+02 -3.341081000000e+02 0.000000000000e+00 7.070493000000e+02 1.805066000000e+02 2.330660000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 3.201153000000e-03 5 | R0_rect: 9.999128000000e-01 1.009263000000e-02 -8.511932000000e-03 -1.012729000000e-02 9.999406000000e-01 -4.037671000000e-03 8.470675000000e-03 4.123522000000e-03 9.999556000000e-01 6 | Tr_velo_to_cam: 6.927964000000e-03 -9.999722000000e-01 -2.757829000000e-03 -2.457729000000e-02 -1.162982000000e-03 2.749836000000e-03 -9.999955000000e-01 -6.127237000000e-02 9.999753000000e-01 6.931141000000e-03 -1.143899000000e-03 -3.321029000000e-01 7 | Tr_imu_to_velo: 9.999976000000e-01 7.553071000000e-04 -2.035826000000e-03 -8.086759000000e-01 -7.854027000000e-04 9.998898000000e-01 -1.482298000000e-02 3.195559000000e-01 2.024406000000e-03 1.482454000000e-02 9.998881000000e-01 -7.997231000000e-01 8 | 9 | -------------------------------------------------------------------------------- /image_2/001665.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yiusay/depth_fusion/75b9d38285e026a9f1ab193a75f43789e0323729/image_2/001665.png -------------------------------------------------------------------------------- /image_2/002893.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yiusay/depth_fusion/75b9d38285e026a9f1ab193a75f43789e0323729/image_2/002893.png -------------------------------------------------------------------------------- /image_2/003969.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yiusay/depth_fusion/75b9d38285e026a9f1ab193a75f43789e0323729/image_2/003969.png -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // main.cpp 3 | // depth_fusion 4 | // 5 | // Created by Yusuke Yasui on 4/23/18. 6 | // Copyright © 2018 Yusuke Yasui. All rights reserved. 7 | // 8 | 9 | #include 10 | #include 11 | #include 12 | #include "opencv2/imgproc.hpp" 13 | #include "opencv2/imgcodecs.hpp" 14 | #include "opencv2/highgui.hpp" 15 | #include "opencv2/calib3d.hpp" 16 | 17 | #include "omp.h" 18 | 19 | //#define DEBUG 20 | 21 | using namespace std; 22 | using namespace cv; 23 | 24 | static float toRadian(const float degree) 25 | { 26 | return degree / 180.0 * M_PI; 27 | } 28 | 29 | static cv::Mat ConvertToHeatmap(const cv::Mat &src, ushort maxVal) 30 | { 31 | cv::Mat srcTrueDepth8U; 32 | src.convertTo(srcTrueDepth8U, CV_8U, 1.0/maxVal * 255.0); 33 | 34 | cv::Mat heatmap; 35 | applyColorMap(srcTrueDepth8U, heatmap, COLORMAP_HSV); 36 | 37 | return heatmap; 38 | } 39 | 40 | // Compute barycentric coordinates (u, v, w) for point p with respect to triangle (a, b, c) 41 | // Copied from https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates 42 | static void Barycentric(Point2f p, Point2f a, Point2f b, Point2f c, float &u, float &v, float &w) 43 | { 44 | Point2f v0 = b - a, v1 = c - a, v2 = p - a; 45 | float d00 = v0.dot(v0); 46 | float d01 = v0.dot(v1); 47 | float d11 = v1.dot(v1); 48 | float d20 = v2.dot(v0); 49 | float d21 = v2.dot(v1); 50 | float denom = d00 * d11 - d01 * d01; 51 | v = (d11 * d20 - d01 * d21) / denom; 52 | w = (d00 * d21 - d01 * d20) / denom; 53 | u = 1.0f - v - w; 54 | } 55 | 56 | class DepthEdgeAlignment 57 | { 58 | cv::Mat srcCamera; 59 | cv::Mat cameraToImage; 60 | std::vector lidarPoints; 61 | cv::Mat edgeWeightX, edgeWeightY; 62 | cv::Mat smoothingWeight; 63 | 64 | cv::Mat InterpolateLidarPoints(const cv::Mat &srcLidar, 65 | std::vector &depthFromLidar, 66 | std::vector &lidarPixelIndex, 67 | cv::Mat &reliableRegion, 68 | int reliableRadius = 5); 69 | 70 | cv::Mat PrimalDualHybridGradient(const cv::Mat &src, double lambda, 71 | const std::vector &depthFromLidar, 72 | const std::vector &lidarPixelIndex, 73 | const cv::Mat &smoothingWeight, 74 | int maxIter = 1000); 75 | 76 | void ProjectOntoUnitBall(cv::Mat &x); 77 | 78 | cv::Mat Aty(const cv::Mat &y, int nRows, int nCols, const cv::Mat &smoothingWeight = cv::Mat()); 79 | cv::Mat Ax(const cv::Mat &x, int nRows, int nCols, const cv::Mat &smoothingWeight = cv::Mat()); 80 | 81 | void GetSmoothingWeights(double tau, double gamma); 82 | 83 | double EstimateCalibrationFidelity(const cv::Mat &estimatedDepth, const cv::Mat &reliableRegion); 84 | 85 | public: 86 | void ReadData(const std::string &cameraPath, 87 | const std::string &lidarPath, 88 | const std::string &calibrationPath, 89 | const std::string &dataId, 90 | cv::Mat &lidarToCamera); 91 | cv::Mat FindCalibrationParameters(const cv::Vec6f &initCalibrationParams, const cv::Vec6f &neighborDelta); 92 | double DepthFusion(const cv::Mat &lidarToCamera, bool isAlignEdge, cv::Mat &optimizedDepth, int nIter = 0); 93 | }; 94 | 95 | 96 | void DepthEdgeAlignment::ReadData(const std::string &cameraPath, 97 | const std::string &lidarPath, 98 | const std::string &calibrationPath, 99 | const std::string &dataId, 100 | cv::Mat &lidarToCamera) 101 | { 102 | ifstream fin; 103 | 104 | // read point clouds from lidar 105 | fin.open(lidarPath + dataId + ".bin", ios::binary); 106 | if (fin) 107 | { 108 | fin.seekg (0, fin.end); 109 | int fileSize = static_cast(fin.tellg()); 110 | fin.seekg (0, fin.beg); 111 | 112 | cerr << "file size " << fileSize << " bytes." << endl; 113 | 114 | CV_Assert(fileSize%16 == 0); 115 | int nPoints = fileSize / 16; // each point consists of 4 floats (= 4 bytes) 116 | 117 | for(int i = 0; i < nPoints; ++i) 118 | { 119 | float x, y, z, reflectance; 120 | 121 | fin.read((char*)&x, sizeof(float)); 122 | fin.read((char*)&y, sizeof(float)); 123 | fin.read((char*)&z, sizeof(float)); 124 | fin.read((char*)&reflectance, sizeof(float)); 125 | 126 | lidarPoints.emplace_back(cv::Point3f(x, y, z)); 127 | } 128 | } 129 | 130 | fin.close(); 131 | 132 | // read calibration data 133 | // For the file format, refer to http://www.mrt.kit.edu/z/publ/download/2013/GeigerAl2013IJRR.pdf 134 | 135 | cv::Mat projectionMat(3, 4, CV_32F), RectifyMat = cv::Mat::zeros(4, 4, CV_32F); 136 | 137 | lidarToCamera = cv::Mat::zeros(4, 4, CV_32F); 138 | 139 | fin.open(calibrationPath + dataId + ".txt"); 140 | if (fin) 141 | { 142 | // extract camera index from cameraPath 143 | CV_Assert(cameraPath.find_last_of("_") != string::npos); 144 | std::string cameraId = "P" + cameraPath.substr(cameraPath.find_last_of("_") + 1, 1); 145 | 146 | cerr << "cameraId " << cameraId << endl; 147 | 148 | auto fillMatrixData = [](cv::Mat m, const std::vector &tokens) 149 | { 150 | int index = 1; 151 | for(int y = 0; y < m.rows; ++y) 152 | { 153 | for(int x = 0; x < m.cols; ++x) 154 | { 155 | m.at(y, x) = stof(tokens[index++]); 156 | } 157 | } 158 | }; 159 | 160 | std::string inputLine; 161 | 162 | while(std::getline(fin, inputLine)) 163 | { 164 | std::vector tokens; 165 | size_t curPos = 0, endPos; 166 | 167 | while ((endPos = inputLine.find(" ", curPos)) != std::string::npos) 168 | { 169 | tokens.emplace_back(inputLine.substr(curPos, endPos - curPos)); 170 | curPos = endPos + 1; 171 | } 172 | 173 | tokens.emplace_back(inputLine.substr(curPos)); 174 | 175 | if(!tokens.empty()) 176 | { 177 | if(!tokens[0].compare(cameraId + ":")) 178 | { 179 | fillMatrixData(projectionMat, tokens); 180 | } 181 | else if(!tokens[0].compare("R0_rect:")) 182 | { 183 | fillMatrixData(RectifyMat(cv::Rect(0, 0, 3, 3)), tokens); 184 | RectifyMat.at(3, 3) = 1.0; 185 | } 186 | else if(!tokens[0].compare("Tr_velo_to_cam:")) 187 | { 188 | fillMatrixData(lidarToCamera(cv::Rect(0, 0, 4, 3)), tokens); 189 | lidarToCamera.at(3, 3) = 1.0; 190 | } 191 | } 192 | } 193 | } 194 | 195 | fin.close(); 196 | 197 | cerr << projectionMat << endl; 198 | cerr << RectifyMat << endl; 199 | cerr << lidarToCamera << endl; 200 | 201 | cameraToImage = projectionMat * RectifyMat; 202 | 203 | srcCamera = imread(cameraPath + dataId + ".png", CV_LOAD_IMAGE_ANYDEPTH); 204 | 205 | // small tau -> smoother. higher tau -> more sensitive to small edges 206 | GetSmoothingWeights(0.1, 2.0); 207 | 208 | #ifdef DEBUG 209 | imwrite("srcCamera.png", srcCamera); 210 | #endif 211 | } 212 | 213 | 214 | 215 | // Optimization based on "An Efficient Primal-Dual Hybrid Gradient Algorithm For Total Variation Image Restoration" 216 | // By Mingqiang Zhu and Tony Chan 217 | cv::Mat DepthEdgeAlignment::PrimalDualHybridGradient(const cv::Mat &src, double lambda, 218 | const std::vector &depthFromLidar, 219 | const std::vector &lidarPixelIndex, 220 | const cv::Mat &smoothingWeight, 221 | int maxIter) 222 | { 223 | // invert lambda since in the original paper, lambda is set to regularize term while, 224 | // in Primal-Dual Hybrid Gradient paper, lambad is set to "faithful to input image" term 225 | lambda = 1.0 / lambda; 226 | 227 | int nRows = src.rows; 228 | int nCols = src.cols; 229 | 230 | cv::Mat y; // optimized output 231 | 232 | src.reshape(0, src.cols * src.rows).convertTo(y, CV_64F); 233 | 234 | CV_Assert(depthFromLidar.size() == lidarPixelIndex.size()); 235 | 236 | cv::Mat x = cv::Mat::zeros(y.rows*2, 1, CV_64F); 237 | 238 | cv::Mat Btz = cv::Mat::zeros(y.size(), y.type()); 239 | 240 | for(size_t i = 0; i < lidarPixelIndex.size(); ++i) 241 | { 242 | Btz.at(lidarPixelIndex[i]) = static_cast(depthFromLidar[i]); 243 | } 244 | 245 | for(int k = 0; k <= maxIter; ++k) // add stop criterion based on paper later.. 246 | { 247 | double tau = 0.2 + 0.08*k; 248 | double theta = 0.5 / tau; 249 | 250 | cv::Mat x1 = x + tau * Aty(y, nRows, nCols, smoothingWeight); 251 | 252 | ProjectOntoUnitBall(x1); 253 | 254 | cv::Mat y1 = y - theta * (Ax(x1, nRows, nCols, smoothingWeight) - lambda*Btz); 255 | 256 | double invOnePlusThetaLambda = 1.0 / (1.0 + theta * lambda); 257 | 258 | for(size_t i = 0; i < lidarPixelIndex.size(); ++i) 259 | { 260 | y1.at(lidarPixelIndex[i]) *= invOnePlusThetaLambda; 261 | } 262 | 263 | y = y1; 264 | x = x1; 265 | } 266 | 267 | cv::Mat optimized; 268 | 269 | y.reshape(0, src.rows).convertTo(optimized, src.type()); 270 | 271 | return optimized; 272 | } 273 | 274 | 275 | void DepthEdgeAlignment::ProjectOntoUnitBall(cv::Mat &x) 276 | { 277 | #pragma omp parallel for 278 | for(int i = 0; i < x.rows; i += 2) 279 | { 280 | double &xx = x.at(i); 281 | double &xy = x.at(i + 1); 282 | 283 | double norm = sqrt(xx*xx + xy*xy); 284 | 285 | if(norm > 1.0) 286 | { 287 | double invNorm = 1.0 / norm; 288 | 289 | xx *= invNorm; 290 | xy *= invNorm; 291 | } 292 | } 293 | } 294 | 295 | cv::Mat DepthEdgeAlignment::Aty(const cv::Mat &y, int nRows, int nCols, const cv::Mat &smoothingWeight) 296 | { 297 | cv::Mat Aty = Mat::zeros(nRows*nCols*2, 1, CV_64F); 298 | 299 | // Not the last row/column 300 | #pragma omp parallel for 301 | for(int j = 0; j < nRows - 1; ++j) 302 | { 303 | int index = j*nCols; 304 | for(int i = 0; i < nCols - 1; ++i) 305 | { 306 | double weight = (!smoothingWeight.empty() ? smoothingWeight.at(index) : 1.0); 307 | 308 | Aty.at(index*2) = (y.at(index+1) - y.at(index)) * weight; 309 | Aty.at(index*2+1) = (y.at(index+nCols) - y.at(index)) * weight; 310 | 311 | ++index; 312 | } 313 | } 314 | 315 | // last row 316 | { 317 | int index = (nRows-1)*nCols; 318 | for(int i = 0; i < nCols - 1; ++i) 319 | { 320 | double weight = (!smoothingWeight.empty() ? smoothingWeight.at(index) : 1.0); 321 | 322 | Aty.at(index*2) = (y.at(index+1) - y.at(index)) * weight; 323 | ++index; 324 | } 325 | } 326 | 327 | // last column 328 | { 329 | int index = nCols - 1; 330 | for(int j = 0; j < nRows - 1; ++j) 331 | { 332 | double weight = (!smoothingWeight.empty() ? smoothingWeight.at(index) : 1.0); 333 | 334 | Aty.at(index*2+1) = (y.at(index+nCols) - y.at(index)) * weight; 335 | index += nCols; 336 | } 337 | } 338 | 339 | return Aty; 340 | } 341 | 342 | cv::Mat DepthEdgeAlignment::Ax(const cv::Mat &x, int nRows, int nCols, const cv::Mat &smoothingWeight) 343 | { 344 | cv::Mat Ax = Mat::zeros(nRows*nCols, 1, CV_64F); 345 | 346 | // Not the last row/column 347 | #pragma omp parallel for 348 | for(int j = 0; j < nRows - 1; ++j) 349 | { 350 | int index = j*nCols; 351 | for(int i = 0; i < nCols - 1; ++i) 352 | { 353 | double weight = (!smoothingWeight.empty() ? smoothingWeight.at(index) : 1.0); 354 | 355 | Ax.at(index) += (-x.at(index*2) - x.at(index*2 + 1)) * weight; 356 | Ax.at(index+1) += x.at(index*2) * weight; 357 | Ax.at(index+nCols) += x.at(index*2 + 1) * weight; 358 | 359 | ++index; 360 | } 361 | } 362 | 363 | // for last row 364 | { 365 | int index = (nRows-1)*nCols; 366 | for(int i = 0; i < nCols - 1; ++i) 367 | { 368 | double weight = (!smoothingWeight.empty() ? smoothingWeight.at(index) : 1.0); 369 | 370 | Ax.at(index) += (-x.at(index*2)) * weight; 371 | Ax.at(index+1) += x.at(index*2) * weight; 372 | ++index; 373 | } 374 | } 375 | 376 | // for last column 377 | { 378 | int index = nCols - 1; 379 | for(int j = 0; j < nRows - 1; ++j) 380 | { 381 | double weight = (!smoothingWeight.empty() ? smoothingWeight.at(index) : 1.0); 382 | 383 | Ax.at(index) += (x.at(index*2 + 1)) * weight; 384 | Ax.at(index+nCols) += x.at(index*2 + 1) * weight; 385 | index += nCols; 386 | } 387 | } 388 | 389 | return Ax; 390 | } 391 | 392 | 393 | double DepthEdgeAlignment::EstimateCalibrationFidelity(const cv::Mat &estimatedDepth, const cv::Mat &reliableRegion) 394 | { 395 | double Ax = 0.0, Ay = 0.0; 396 | double edgeWeightSumX = 0.0, edgeWeightSumY = 0.0; 397 | double depthGradSumX = 0.0, depthGradSumY = 0.0; 398 | 399 | for(int y = 0; y < estimatedDepth.rows - 1; ++y) 400 | { 401 | const double* d1 = estimatedDepth.ptr(y); 402 | const double* d2 = estimatedDepth.ptr(y + 1); 403 | const double* ewx = edgeWeightX.ptr(y); 404 | const double* ewy = edgeWeightY.ptr(y); 405 | const uchar* r = reliableRegion.ptr(y); 406 | 407 | for(int x = 0; x < estimatedDepth.cols - 1; ++x) 408 | { 409 | if(*r++ > 0) 410 | { 411 | // only consider region surronded by lidar data 412 | double gradX = fabs(*(d1 + 1) - *d1); 413 | double gradY = fabs(*d2 - *d1); 414 | 415 | Ax += (*ewx * gradX); 416 | edgeWeightSumX += *ewx; 417 | depthGradSumX += gradX; 418 | 419 | Ay += (*ewy * gradY); 420 | edgeWeightSumY += *ewy; 421 | depthGradSumY += gradY; 422 | } 423 | 424 | ++d1; 425 | ++d2; 426 | ++ewx; 427 | ++ewy; 428 | } 429 | } 430 | 431 | return Ax / (edgeWeightSumX * depthGradSumX) + Ay / (edgeWeightSumY * depthGradSumY); 432 | } 433 | 434 | cv::Mat DepthEdgeAlignment::InterpolateLidarPoints(const cv::Mat &srcLidar, 435 | std::vector &depthFromLidar, 436 | std::vector &lidarPixelIndex, 437 | cv::Mat &reliableRegion, 438 | int reliableRadius) 439 | { 440 | cv::Rect imgBoundary(0, 0, srcLidar.cols, srcLidar.rows); 441 | 442 | cv::Subdiv2D subdiv(imgBoundary); 443 | 444 | for(int i = 0; i < srcLidar.rows; i++) 445 | { 446 | const double* p = srcLidar.ptr(i); 447 | int index = i*srcLidar.cols; 448 | 449 | for(int j = 0; j < srcLidar.cols; j++) 450 | { 451 | if(*p > 0) 452 | { 453 | subdiv.insert(Point2f(j, i)); 454 | 455 | depthFromLidar.emplace_back(*p); 456 | lidarPixelIndex.emplace_back(index); 457 | 458 | cv::circle(reliableRegion, Point(j, i), reliableRadius, cv::Scalar(255), -1); 459 | } 460 | 461 | ++p; 462 | ++index; 463 | } 464 | } 465 | 466 | cv::erode(reliableRegion, reliableRegion, getStructuringElement(cv::MORPH_ELLIPSE, Size(reliableRadius*2, reliableRadius*2))); 467 | 468 | 469 | cv::Mat srcLidarInterpolated = cv::Mat::zeros(srcLidar.size(), srcLidar.type()); 470 | 471 | for(int i = 0; i < srcLidar.rows; i++) 472 | { 473 | const double* src = srcLidar.ptr(i); 474 | double* dst = srcLidarInterpolated.ptr(i); 475 | 476 | for(int j = 0; j < srcLidar.cols; j++) 477 | { 478 | if(*src > 0) 479 | { 480 | *dst = *src; 481 | } 482 | else 483 | { 484 | // data is missing in src. Interepolate it from the neighbors 485 | Point2f targetPoint(j, i); 486 | Point2f neighborPoint[3]; 487 | double neighborDepth[3]; 488 | 489 | int edgeId, vertexId; 490 | int pointLoc = subdiv.locate(targetPoint, edgeId, vertexId); 491 | 492 | for(int k = 0; k < 3; k++) 493 | { 494 | neighborPoint[k] = subdiv.getVertex(subdiv.edgeOrg(edgeId)); 495 | 496 | edgeId = subdiv.getEdge(edgeId, Subdiv2D::NEXT_AROUND_LEFT); 497 | } 498 | 499 | vector isPointValid(3, false); 500 | 501 | for(int k = 0; k < 3; ++k) 502 | { 503 | if(imgBoundary.contains(neighborPoint[k])) 504 | { 505 | neighborDepth[k] = srcLidar.at(neighborPoint[k].y, neighborPoint[k].x); 506 | isPointValid[k] = true; 507 | } 508 | } 509 | 510 | bool isAllPointsValid = (isPointValid[0] && isPointValid[1] && isPointValid[2]); 511 | 512 | if(isAllPointsValid) 513 | { 514 | float u, v, w; 515 | Barycentric(targetPoint, neighborPoint[0], neighborPoint[1], neighborPoint[2], u, v, w); 516 | 517 | *dst = neighborDepth[0]*u + neighborDepth[1]*v + neighborDepth[2]*w; 518 | } 519 | } 520 | 521 | ++src; 522 | ++dst; 523 | } 524 | } 525 | 526 | return srcLidarInterpolated; 527 | } 528 | 529 | static cv::Mat GetLidarToCamera(cv::Vec6f &calibrationParams) 530 | { 531 | cv::Mat lidarToCamera = cv::Mat::zeros(4, 4, CV_32F); 532 | 533 | cv::Vec6f &cp = calibrationParams; 534 | 535 | double length = sqrt(cp[0]*cp[0] + cp[1]*cp[1]); 536 | if(length >= 1.0) 537 | { 538 | cp[0] /= length; 539 | cp[1] /= length; 540 | } 541 | 542 | cv::Mat rotationAxis = (Mat_(3, 1) << cp[0], cp[1], sqrt(1.0 - cp[0]*cp[0] - cp[1]*cp[1])); 543 | 544 | Rodrigues(rotationAxis * cp[2], lidarToCamera(cv::Rect(0, 0, 3, 3))); 545 | 546 | lidarToCamera.at(0, 3) = cp[3]; 547 | lidarToCamera.at(1, 3) = cp[4]; 548 | lidarToCamera.at(2, 3) = cp[5]; 549 | lidarToCamera.at(3, 3) = 1.0f; 550 | 551 | return lidarToCamera; 552 | } 553 | 554 | 555 | void DepthEdgeAlignment::GetSmoothingWeights(double tau, double gamma) 556 | { 557 | edgeWeightX = Mat::zeros(srcCamera.size(), CV_64F); 558 | edgeWeightY = Mat::zeros(srcCamera.size(), CV_64F); 559 | 560 | for(int y = 0; y < srcCamera.rows; y++) 561 | { 562 | const uchar* p = srcCamera.ptr(y); 563 | double* wx = edgeWeightX.ptr(y); 564 | 565 | for(int x = 0; x < srcCamera.cols - 1; x++) 566 | { 567 | *wx++ = *(p+1) - *p; 568 | ++p; 569 | } 570 | } 571 | 572 | for(int y = 0; y < srcCamera.rows - 1; y++) 573 | { 574 | const uchar* p1 = srcCamera.ptr(y); 575 | const uchar* p2 = srcCamera.ptr(y+1); 576 | double* wy = edgeWeightY.ptr(y); 577 | 578 | for(int x = 0; x < srcCamera.cols; x++) 579 | { 580 | *wy++ = *p2 - *p1; 581 | ++p1; 582 | ++p2; 583 | } 584 | } 585 | 586 | smoothingWeight = Mat::zeros(srcCamera.size(), CV_64F); 587 | 588 | CV_Assert(smoothingWeight.size() == edgeWeightX.size()); 589 | CV_Assert(smoothingWeight.size() == edgeWeightY.size()); 590 | 591 | for(int y = 0; y < smoothingWeight.rows; y++) 592 | { 593 | double* wx = edgeWeightX.ptr(y); 594 | double* wy = edgeWeightY.ptr(y); 595 | double* w = smoothingWeight.ptr(y); 596 | 597 | for(int x = 0; x < smoothingWeight.cols; x++) 598 | { 599 | *w++ = exp(-tau * sqrt((*wx) * (*wx) + (*wy) * (*wy))); 600 | 601 | *wx = exp(-gamma * fabs(*wx)); 602 | *wy = exp(-gamma * fabs(*wy)); 603 | 604 | ++wx; 605 | ++wy; 606 | } 607 | } 608 | 609 | } 610 | 611 | 612 | cv::Mat DepthEdgeAlignment::FindCalibrationParameters(const cv::Vec6f &calibrationParams, 613 | const cv::Vec6f &neighborDelta) 614 | { 615 | ofstream fout("iteration.csv"); 616 | 617 | RNG rng; 618 | 619 | double minFidelity = std::numeric_limits::max(); 620 | cv::Vec6f bestCalibParams; 621 | 622 | cv::Mat depthMap; 623 | 624 | // run simulated-annealing 625 | const double minTemperature = 0.0007; 626 | const double alpha = 0.9; 627 | double temperature = 0.43; 628 | 629 | 630 | cv::Vec6f prevCalibrationParams = calibrationParams; 631 | double prevCalibrationFidelity = DepthFusion(GetLidarToCamera(prevCalibrationParams), 632 | false, // this disables edge-aware optimization 633 | depthMap, 634 | 0); 635 | 636 | bestCalibParams = prevCalibrationParams; 637 | minFidelity = prevCalibrationFidelity; 638 | 639 | int nIter = 1; 640 | 641 | while(temperature > minTemperature) 642 | { 643 | for(int k = 0; k < 10; ++k) 644 | { 645 | // update one of six calibration parameters per iteration 646 | int whichParam = rng.uniform(0, 6); 647 | 648 | cv::Vec6f newCalibrationParams = prevCalibrationParams; 649 | newCalibrationParams[whichParam] += neighborDelta[whichParam] * rng.uniform(-1.0f, 1.0f); 650 | 651 | double newCalibrationFidelity = DepthFusion(GetLidarToCamera(newCalibrationParams), 652 | false, // this disables edge-aware optimization 653 | depthMap, 654 | nIter); 655 | 656 | if(newCalibrationFidelity < minFidelity) 657 | { 658 | minFidelity = newCalibrationFidelity; 659 | bestCalibParams = newCalibrationParams; 660 | } 661 | 662 | cerr << "iter " << nIter << " cost " << newCalibrationFidelity << " " << newCalibrationParams[0] << " " << newCalibrationParams[1] << " " << newCalibrationParams[2]/M_PI*180.0 663 | << "\n" << GetLidarToCamera(newCalibrationParams) << endl; 664 | 665 | double costDelta = newCalibrationFidelity - prevCalibrationFidelity; 666 | 667 | double probability = exp(-costDelta*1000000 / temperature); 668 | double randomVal = rng.uniform(0.0, 1.0); 669 | 670 | #ifdef DEBUG 671 | cerr << costDelta << " " << probability << " " << randomVal << endl; 672 | #endif 673 | 674 | if(costDelta < 0.0 || probability > randomVal) 675 | { 676 | cerr << "updated\n"; 677 | prevCalibrationParams = newCalibrationParams; 678 | prevCalibrationFidelity = newCalibrationFidelity; 679 | } 680 | 681 | fout << nIter << "," << prevCalibrationFidelity << ","; 682 | for(int i = 0; i < 6; i++) 683 | { 684 | fout << prevCalibrationParams[i] << ","; 685 | } 686 | fout << endl; 687 | 688 | nIter++; 689 | } 690 | 691 | temperature *= alpha; 692 | } 693 | 694 | cerr << "best calib params " << bestCalibParams << " min cost " << minFidelity << endl; 695 | 696 | cv::Mat bestLidarToCamera = GetLidarToCamera(bestCalibParams); 697 | 698 | cerr << "best LidarToCamera:\n"; 699 | cerr << bestLidarToCamera << endl; 700 | 701 | fout.close(); 702 | 703 | return bestLidarToCamera; 704 | } 705 | 706 | double DepthEdgeAlignment::DepthFusion(const cv::Mat &lidarToCamera, 707 | bool isAlignEdge, 708 | cv::Mat &optimizedDepth, // this is output 709 | int nIter) 710 | { 711 | cv::Mat projectedLidarPoints = cv::Mat::zeros(srcCamera.size(), CV_64F); 712 | 713 | #pragma omp parallel for 714 | for(size_t i = 0; i < lidarPoints.size(); ++i) 715 | { 716 | cv::Mat lidarCoord = (Mat_(4, 1) << lidarPoints[i].x, lidarPoints[i].y, lidarPoints[i].z, 1); 717 | 718 | cv::Mat cameraCoord = lidarToCamera * lidarCoord; 719 | 720 | float depth = cameraCoord.at(2); 721 | 722 | cv::Mat imageCoord = cameraToImage * cameraCoord; 723 | float invNormalize = 1.0 / imageCoord.at(2); 724 | 725 | cv::Point normalizedImgCoord(imageCoord.at(0) * invNormalize, imageCoord.at(1) * invNormalize); 726 | 727 | if(cv::Rect(0, 0, srcCamera.cols, srcCamera.rows).contains(normalizedImgCoord) && depth > 0.0f) 728 | { 729 | projectedLidarPoints.at(normalizedImgCoord.y, normalizedImgCoord.x) = depth; 730 | } 731 | } 732 | 733 | #ifdef DEBUG 734 | imwrite("projectedLidarPoints_" + to_string(nIter) + ".png", ConvertToHeatmap(projectedLidarPoints, 80.0)); 735 | imwrite("srcCamera.png", srcCamera); 736 | #endif 737 | 738 | std::vector depthFromLidar; 739 | 740 | // store the index of pixel (in 1D) from which the corresponding lidar data comes 741 | std::vector lidarPixelIndex; 742 | 743 | // this is the area where we measure the error 744 | cv::Mat reliableRegion = cv::Mat::zeros(srcCamera.size(), CV_8U); 745 | 746 | cv::Mat srcLidarInterpolated = InterpolateLidarPoints(projectedLidarPoints, 747 | depthFromLidar, 748 | lidarPixelIndex, 749 | reliableRegion, 750 | 5); 751 | 752 | #ifdef DEBUG 753 | imwrite("srcLidarInterpolated_" + to_string(nIter) + ".png", ConvertToHeatmap(srcLidarInterpolated, 80.0)); 754 | imwrite("reliableRegion_" + to_string(nIter) + ".png", reliableRegion); 755 | #endif 756 | 757 | optimizedDepth = PrimalDualHybridGradient(srcLidarInterpolated, 2, depthFromLidar, lidarPixelIndex, 758 | isAlignEdge ? smoothingWeight : cv::Mat(), // disable smoothing weight during calibration 759 | isAlignEdge ? 300 : 100); 760 | 761 | #ifdef DEBUG 762 | imwrite("optimized_" + to_string(nIter) + ".png", ConvertToHeatmap(optimizedDepth, 80)); 763 | #endif 764 | 765 | return EstimateCalibrationFidelity(optimizedDepth, reliableRegion); 766 | } 767 | 768 | int main(int argc, char ** argv) 769 | { 770 | DepthEdgeAlignment edgeAlign; 771 | 772 | cv::Mat lidarToCamera; // This is the "true" extrinsic camera matrix 773 | 774 | edgeAlign.ReadData("image_2/", 775 | "velodyne/", 776 | "calib/", 777 | "001665",//"002893"; // 003969 // 001665 778 | lidarToCamera); 779 | 780 | 781 | // Set "reCalibrate" to true if you need to re-calculate calibration data. 782 | // The extrinsic camera matrix (=lidarToCamera) is updated using simulated-annealing 783 | bool reCalibrate = false; 784 | 785 | if(reCalibrate) 786 | { 787 | cv::Mat trueRotationVector; 788 | Rodrigues(lidarToCamera(cv::Rect(0, 0, 3, 3)), trueRotationVector); 789 | 790 | float trueAngle = sqrt(trueRotationVector.dot(trueRotationVector)); 791 | trueRotationVector *= (1.0 / trueAngle); 792 | 793 | cerr << "true angle " << trueAngle/M_PI*180.0 << endl; 794 | cerr << "true rotation vector\n" << trueRotationVector << endl; 795 | cerr << "true translation\n" << lidarToCamera.at(0, 3) << " " << lidarToCamera.at(1, 3) << " " << lidarToCamera.at(2, 3) << endl; 796 | 797 | cv::Vec6f neighborDelta = { 0.001, 0.001, toRadian(3.0), 0.01, 0.01, 0.001 }; 798 | 799 | // rot_axis_X, rot_axis_Y, angle, transX, transY, transZ 800 | cv::Vec6f initCalibrationParams(trueRotationVector.at(0), 801 | trueRotationVector.at(1), 802 | trueAngle, 803 | lidarToCamera.at(0, 3), 804 | lidarToCamera.at(1, 3), 805 | lidarToCamera.at(2, 3)); 806 | 807 | 808 | cv::Mat depthMap; 809 | cerr << "true fidelity " << edgeAlign.DepthFusion(GetLidarToCamera(initCalibrationParams), 810 | false, // this disables edge-aware optimization 811 | depthMap, 812 | -1) << endl; 813 | 814 | // perturb calibration data for test 815 | cv::RNG rng; 816 | cv::Vec6f &cp = initCalibrationParams; 817 | 818 | for(int i = 0; i < 6; ++i) 819 | { 820 | cp[i] += neighborDelta[i] * rng.uniform(-1.0f, 1.0f) * 3.0; 821 | } 822 | 823 | lidarToCamera = edgeAlign.FindCalibrationParameters(cp, neighborDelta); 824 | } 825 | 826 | // edge-aware depth map optimization using "true/updated" calibration data 827 | cv::Mat edgeAlignedDepth; 828 | edgeAlign.DepthFusion(lidarToCamera, true, edgeAlignedDepth, 0); 829 | 830 | imwrite("edgeAlignedDepth.png", ConvertToHeatmap(edgeAlignedDepth, 80.0)); 831 | 832 | return 0; 833 | } 834 | -------------------------------------------------------------------------------- /velodyne/001665.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yiusay/depth_fusion/75b9d38285e026a9f1ab193a75f43789e0323729/velodyne/001665.bin -------------------------------------------------------------------------------- /velodyne/002893.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yiusay/depth_fusion/75b9d38285e026a9f1ab193a75f43789e0323729/velodyne/002893.bin -------------------------------------------------------------------------------- /velodyne/003969.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yiusay/depth_fusion/75b9d38285e026a9f1ab193a75f43789e0323729/velodyne/003969.bin --------------------------------------------------------------------------------