hMatrix Jacobian_hMatrix(double *theta, double *alpha, double *a, double *d) { hMatrix T01(4,4), T02(4,4), T03(4,4), T04(4,4), T05(4,4), T06(4,4), T07(4,4); T01 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 1); T02 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 2); T03 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 3); T04 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 4); T05 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 5); T06 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 6); T07 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 7); double k[3] = {0,0,1}; double z1[3] = { T01.element(0,2),T01.element(1,2), T01.element(2,2)}; double z2[3] = { T02.element(0,2),T02.element(1,2), T02.element(2,2)}; double z3[3] = { T03.element(0,2),T03.element(1,2), T03.element(2,2)}; double z4[3] = { T04.element(0,2),T04.element(1,2), T04.element(2,2)}; double z5[3] = { T05.element(0,2),T05.element(1,2), T05.element(2,2)}; double z6[3] = { T06.element(0,2),T06.element(1,2), T06.element(2,2)}; double o1[3] = {T01.element(0,3), T01.element(1,3), T01.element(2,3)}; double o2[3] = {T02.element(0,3), T02.element(1,3), T02.element(2,3)}; double o3[3] = {T03.element(0,3), T03.element(1,3), T03.element(2,3)}; double o4[3] = {T04.element(0,3), T04.element(1,3), T04.element(2,3)}; double o5[3] = {T05.element(0,3), T05.element(1,3), T05.element(2,3)}; double o6[3] = {T06.element(0,3), T06.element(1,3), T06.element(2,3)}; double o7[3] = {T07.element(0,3), T07.element(1,3), T07.element(2,3)}; double O1[3] ={o7[0],o7[1],o7[2]}; double O2[3] ={o7[0]-o1[0],o7[1]-o1[1],o7[2]-o1[2]}; double O3[3] ={o7[0]-o2[0],o7[1]-o2[1],o7[2]-o2[2]}; double O4[3] ={o7[0]-o3[0],o7[1]-o3[1],o7[2]-o3[2]}; double O5[3] ={o7[0]-o4[0],o7[1]-o4[1],o7[2]-o4[2]}; double O6[3] ={o7[0]-o5[0],o7[1]-o5[1],o7[2]-o5[2]}; double O7[3] ={o7[0]-o6[0],o7[1]-o6[1],o7[2]-o6[2]}; hMatrix c1(1,3),c2(1,3),c3(1,3),c4(1,3),c5(1,3),c6(1,3),c7(1,3); c1 = cross(&k[0],&O1[0]); c2 = cross(&z1[0],&O2[0]); c3 = cross(&z2[0],&O3[0]); c4 = cross(&z3[0],&O4[0]); c5 = cross(&z4[0],&O5[0]); c6 = cross(&z5[0],&O6[0]); c7 = cross(&z6[0],&O7[0]); double J[42] = { c1.element(0,0), c2.element(0,0), c3.element(0,0), c4.element(0,0), c5.element(0,0), c6.element(0,0), c7.element(0,0), c1.element(0,1), c2.element(0,1), c3.element(0,1), c4.element(0,1), c5.element(0,1), c6.element(0,1), c7.element(0,1), c1.element(0,2), c2.element(0,2), c3.element(0,2), c4.element(0,2), c5.element(0,2), c6.element(0,2), c7.element(0,2), k[0],z1[0],z2[0],z3[0],z4[0],z5[0],z6[0], k[1],z1[1],z2[1],z3[1],z4[1],z5[1],z6[1], k[2],z1[2],z2[2],z3[2],z4[2],z5[2],z6[2]}; hMatrix Jacobian(6,7); Jacobian.SET(6,7,&J[0]); return Jacobian; }
enum parseType T() { char *save = next; if (!T04()) { next = save; if (!T02()) { next = save; if (!T03()) { next = save; if (!T01()) { return tERR; } else return tT01; } else return tT03; } else return tT02; } else return tT04; return tERR; }
int main(int argc, char** argv) { /*Polynomial2 poly2; poly2.kuu = -1; poly2.kuv = 1; poly2.kvv= -1; poly2.ku = 0.25; poly2.kv = 0.25; poly2.k1 = 5; CurveRasterizer<Polynomial2> raster(1, 1, -100, 100, poly2); CurveRasterizer2<Polynomial2> raster2(1, 1, -100, 100, poly2); auto tr0 = clock(); int x1 = 0; int x2 = 0; for (int i = 0; i < 10000000; i++) { raster.step(); x1 += raster.x; } auto tr1 = clock(); for (int i = 0; i < 10000000; i++) { raster2.step(); x2 += raster2.x; } auto tr2 = clock(); cout << "optimized " << double(tr1 - tr0) / CLOCKS_PER_SEC << endl; cout << "simple " << double(tr2 - tr1) / CLOCKS_PER_SEC << endl; cout << x1 << " " << x2 << endl; return 0;*/ ifstream paramFile(argv[1]); if (not paramFile.is_open()) { cout << argv[1] << " : ERROR, file is not found" << endl; return 0; } array<double, 6> params1; array<double, 6> params2; cout << "First EU Camera model parameters :" << endl; for (auto & p: params1) { paramFile >> p; cout << setw(10) << p; } cout << endl; paramFile.ignore(); cout << "Second EU Camera model parameters :" << endl; for (auto & p: params2) { paramFile >> p; cout << setw(10) << p; } cout << endl; paramFile.ignore(); array<double, 6> cameraPose; cout << "Camera pose wrt the robot :" << endl; for (auto & e: cameraPose) { paramFile >> e; cout << setw(10) << e; } cout << endl; paramFile.ignore(); Transformation<double> TbaseCamera(cameraPose.data()); array<double, 6> robotPose1, robotPose2; cout << "First robot's pose :" << endl; for (auto & e: robotPose1) { paramFile >> e; cout << setw(10) << e; } cout << endl; cout << "Second robot's pose :" << endl; for (auto & e: robotPose2) { paramFile >> e; cout << setw(10) << e; } cout << endl; paramFile.ignore(); Transformation<double> T01(robotPose1.data()), T02(robotPose2.data()); Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera)); SGMParameters stereoParams; stereoParams.flawCost = 5; stereoParams.verbosity = 0; stereoParams.hypMax = 1; // stereoParams.salientPoints = false; paramFile >> stereoParams.u0; paramFile >> stereoParams.v0; paramFile >> stereoParams.dispMax; paramFile >> stereoParams.scale; paramFile.ignore(); // stereoParams.lambdaStep = 3; // stereoParams.lambdaJump = 15; string fileName1, fileName2; while(getline(paramFile, fileName1)) { getline(paramFile, fileName2); Mat8u img1 = imread(fileName1, 0); Mat8u img2 = imread(fileName2, 0); Mat16s img1lap, img2lap; stereoParams.uMax = img1.cols; stereoParams.vMax = img1.rows; stereoParams.setEqualMargin(); // // Laplacian(img1, img1lap, CV_16S, 3); // Laplacian(img2, img2lap, CV_16S, 3); // // GaussianBlur(img1, img1, Size(3, 3), 0, 0); // GaussianBlur(img2, img2, Size(3, 3), 0, 0); // // img1lap = img1lap + 128; // img2lap = img2lap + 128; // img1lap.copyTo(img1); // img2lap.copyTo(img2); //// Timer timer; EnhancedCamera camera1(params1.data()), camera2(params2.data()); EnhancedSGM stereo(TleftRight, &camera1, &camera2, stereoParams); cout << " initialization time : " << timer.elapsed() << endl; Mat8u out1, out2; img1.copyTo(out1); img2.copyTo(out2); // // for (auto & x : {Point(320, 300), Point(500, 300), Point(750, 300), Point(350, 500), Point(600, 450)}) // { // out1(x) = 0; // out1(x.y + 1, x.x) = 0; // out1(x.y, x.x + 1) = 255; // out1(x.y + 1, x.x + 1) = 255; // stereo.traceEpipolarLine(x, out2); // } // Mat32f res; // timer.reset(); // stereo.computeCurveCost(img1, img2); // cout << timer.elapsed() << endl; // timer.reset(); // stereo.computeDynamicProgramming(); // cout << timer.elapsed() << endl; // timer.reset(); // stereo.reconstructDisparity(); // cout << timer.elapsed() << endl; // timer.reset(); // stereo.computeDepth(res); // cout << timer.elapsed() << endl; DepthMap depth; Mat32f depthMat; timer.reset(); // stereo.computeStereo(img1, img2, depthMat); stereo.computeStereo(img1, img2, depth); cout << " stereo total time : " << timer.elapsed() << endl; depth.toInverseMat(depthMat); imshow("out1", out1); imshow("out2", out2); imshow("res", depthMat/ 3); imshow("disp", stereo.disparity() * 256); cout << stereo.disparity()(350, 468) << " " << stereo.disparity()(350, 469) << endl; waitKey(); } return 0; }
int main(int argc, char** argv) { ifstream paramFile(argv[1]); if (not paramFile.is_open()) { cout << argv[1] << " : ERROR, file is not found" << endl; return 0; } array<double, 6> params1; array<double, 6> params2; cout << "First EU Camera model parameters :" << endl; for (auto & p: params1) { paramFile >> p; cout << setw(10) << p; } cout << endl; paramFile.ignore(); cout << "Second EU Camera model parameters :" << endl; for (auto & p: params2) { paramFile >> p; cout << setw(10) << p; } cout << endl; paramFile.ignore(); array<double, 6> cameraPose; cout << "Camera pose wrt the robot :" << endl; for (auto & e: cameraPose) { paramFile >> e; cout << setw(10) << e; } cout << endl; paramFile.ignore(); Transformation<double> TbaseCamera(cameraPose.data()); array<double, 6> robotPose1, robotPose2; cout << "First robot's pose :" << endl; for (auto & e: robotPose1) { paramFile >> e; cout << setw(10) << e; } cout << endl; cout << "Second robot's pose :" << endl; for (auto & e: robotPose2) { paramFile >> e; cout << setw(10) << e; } cout << endl; paramFile.ignore(); Transformation<double> T01(robotPose1.data()), T02(robotPose2.data()); Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera)); EnhancedCamera cam1(params1.data()), cam2(params2.data()); EnhancedEpipolar epipolar(&cam1, &cam2, TleftRight, 2000); string fileName1, fileName2; getline(paramFile, fileName1); //to SGM parameters const int LENGTH = 5; //TODO fix the constructor to avoid NULL EpipolarDescriptor epipolarDescriptor(LENGTH, 3, {1, 2, 3, 5, 7, 9}); StereoEpipoles epipoles(&cam1, &cam2, TleftRight); while(getline(paramFile, fileName1)) { getline(paramFile, fileName2); img1 = imread(fileName1, 0); img2 = imread(fileName2, 0); Mat8u descStepMat(img1.size()); for (int v = 0; v < img1.rows; v++) { for (int u = 0; u < img1.cols; u++) { Vector3d X; cam1.reconstructPoint(Vector2d(u, v), X); CurveRasterizer<int, Polynomial2> raster(Vector2i(u, v), epipoles.getFirstPx(), epipolar.getFirst(X)); if (epipoles.firstIsInverted()) raster.setStep(-1); vector<uint8_t> descriptor; const int step = epipolarDescriptor.compute(img1, raster, descriptor); if (step > 0) descStepMat(v, u) = (10 - step) * 25; else descStepMat(v, u) = 0; } } imshow("out1", img1); imshow("out2", img2); imshow("descStep", descStepMat); waitKey(); } return 0; }
int main(int argc, char** argv) { /*Polynomial2 poly2; poly2.kuu = -1; poly2.kuv = 1; poly2.kvv= -1; poly2.ku = 0.25; poly2.kv = 0.25; poly2.k1 = 5; CurveRasterizer<Polynomial2> raster(1, 1, -100, 100, poly2); CurveRasterizer2<Polynomial2> raster2(1, 1, -100, 100, poly2); auto tr0 = clock(); int x1 = 0; int x2 = 0; for (int i = 0; i < 10000000; i++) { raster.step(); x1 += raster.x; } auto tr1 = clock(); for (int i = 0; i < 10000000; i++) { raster2.step(); x2 += raster2.x; } auto tr2 = clock(); cout << "optimized " << double(tr1 - tr0) / CLOCKS_PER_SEC << endl; cout << "simple " << double(tr2 - tr1) / CLOCKS_PER_SEC << endl; cout << x1 << " " << x2 << endl; return 0;*/ ifstream paramFile(argv[1]); if (not paramFile.is_open()) { cout << argv[1] << " : ERROR, file is not found" << endl; return 0; } array<double, 6> params; cout << "EU Camera model parameters :" << endl; for (auto & p: params) { paramFile >> p; cout << setw(10) << p; } cout << endl; paramFile.ignore(); array<double, 6> cameraPose; cout << "Camera pose wrt the robot :" << endl; for (auto & e: cameraPose) { paramFile >> e; cout << setw(10) << e; } cout << endl; paramFile.ignore(); Transformation<double> TbaseCamera(cameraPose.data()); array<double, 6> robotPose1, robotPose2; SGMParameters stereoParams; stereoParams.verbosity = 3; stereoParams.salientPoints = false; paramFile >> stereoParams.u0; paramFile >> stereoParams.v0; paramFile >> stereoParams.dispMax; paramFile >> stereoParams.scale; paramFile.ignore(); string imageDir; getline(paramFile, imageDir); string imageInfo, imageName; getline(paramFile, imageInfo); istringstream imageStream(imageInfo); imageStream >> imageName; for (auto & x : robotPose1) imageStream >> x; Mat8u img1 = imread(imageDir + imageName, 0); stereoParams.uMax = img1.cols; stereoParams.vMax = img1.rows; stereoParams.setEqualMargin(); int counter = 2; EnhancedCamera camera(params.data()); while (getline(paramFile, imageInfo)) { istringstream imageStream(imageInfo); imageStream >> imageName; for (auto & x : robotPose2) imageStream >> x; Transformation<double> T01(robotPose1.data()), T02(robotPose2.data()); Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera)); Mat8u img2 = imread(imageDir + imageName, 0); EnhancedSGM stereo(TleftRight, &camera, &camera, stereoParams); DepthMap depth; auto t2 = clock(); stereo.computeStereo(img1, img2, depth); auto t3 = clock(); cout << double(t3 - t2) / CLOCKS_PER_SEC << endl; Mat32f dMat; depth.toInverseMat(dMat); // imwrite(imageDir + "res" + to_string(counter++) + ".png", depth*200); imwrite(imageDir + "res" + to_string(counter++) + ".png", dMat*30); } return 0; }
int main(int argc, char** argv) { /*Polynomial2 poly2; poly2.kuu = -1; poly2.kuv = 1; poly2.kvv= -1; poly2.ku = 0.25; poly2.kv = 0.25; poly2.k1 = 5; CurveRasterizer<Polynomial2> raster(1, 1, -100, 100, poly2); CurveRasterizer2<Polynomial2> raster2(1, 1, -100, 100, poly2); auto tr0 = clock(); int x1 = 0; int x2 = 0; for (int i = 0; i < 10000000; i++) { raster.step(); x1 += raster.x; } auto tr1 = clock(); for (int i = 0; i < 10000000; i++) { raster2.step(); x2 += raster2.x; } auto tr2 = clock(); cout << "optimized " << double(tr1 - tr0) / CLOCKS_PER_SEC << endl; cout << "simple " << double(tr2 - tr1) / CLOCKS_PER_SEC << endl; cout << x1 << " " << x2 << endl; return 0;*/ ifstream paramFile(argv[1]); if (not paramFile.is_open()) { cout << argv[1] << " : ERROR, file is not found" << endl; return 0; } array<double, 6> params; cout << "EU Camera model parameters :" << endl; for (auto & p: params) { paramFile >> p; cout << setw(10) << p; } cout << endl; paramFile.ignore(); array<double, 6> cameraPose; cout << "Camera pose wrt the robot :" << endl; for (auto & e: cameraPose) { paramFile >> e; cout << setw(10) << e; } cout << endl; paramFile.ignore(); Transformation<double> TbaseCamera(cameraPose.data()); array<double, 6> robotPose1, robotPose2; SGMParameters stereoParams2; stereoParams2.salientPoints = false; stereoParams2.verbosity = 3; stereoParams2.hypMax = 1; // stereoParams.salientPoints = false; paramFile >> stereoParams2.u0; paramFile >> stereoParams2.v0; paramFile >> stereoParams2.dispMax; paramFile >> stereoParams2.scale; paramFile.ignore(); string imageDir; getline(paramFile, imageDir); string imageInfo, imageName; getline(paramFile, imageInfo); istringstream imageStream(imageInfo); imageStream >> imageName; for (auto & x : robotPose1) imageStream >> x; Mat8u img1 = imread(imageDir + imageName, 0); cout << "Image name: "<< imageDir + imageName << endl; cout << "Image size: "<<img1.size()<<endl;; stereoParams2.u0 = 50; stereoParams2.v0 = 50; stereoParams2.uMax = img1.cols; stereoParams2.vMax = img1.rows; stereoParams2.setEqualMargin(); // stereoParams2.salientPoints = true; int counter = 2; EnhancedCamera camera(params.data()); DepthMap depth; depth.setDefault(); // stereoParams2.dispMax = 40; // stereoParams2.descRespThresh = 2; // stereoParams2.scaleVec = vector<int>{1}; MotionStereoParameters stereoParams(stereoParams2); stereoParams.verbosity = 1; stereoParams.descLength = 5; // stereoParams.descRespThresh = 2; // stereoParams.scaleVec = vector<int>{1}; MotionStereo stereo(&camera, &camera, stereoParams); stereo.setBaseImage(img1); //do SGM to init the depth getline(paramFile, imageInfo); getline(paramFile, imageInfo); getline(paramFile, imageInfo); imageStream.str(imageInfo); imageStream.clear(); imageStream >> imageName; for (auto & x : robotPose2) imageStream >> x; Mat8u img2 = imread(imageDir + imageName, 0); Transformation<double> T01(robotPose1.data()), T02(robotPose2.data()); Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera)); EnhancedSGM stereoSG(TleftRight, &camera, &camera, stereoParams2); Timer timer; stereoSG.computeStereo(img1, img2, depth); depth.filterNoise(); cout << timer.elapsed() << endl; Mat32f res, sigmaRes; Mat32f res2, sigmaRes2; depth.toInverseMat(res2); depth.sigmaToMat(sigmaRes2); imshow("res" + to_string(counter), res2 *0.12); imshow("sigma " + to_string(counter), sigmaRes2*20); cv::waitKey(0); counter++; while (getline(paramFile, imageInfo)) { istringstream imageStream(imageInfo); imageStream >> imageName; for (auto & x : robotPose2) imageStream >> x; Transformation<double> T01(robotPose1.data()), T02(robotPose2.data()); Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera)); Mat8u img2 = imread(imageDir + imageName, 0); // depth.setDefault(); timer.reset(); cout << TleftRight << endl; DepthMap depth2 = stereo.compute(TleftRight, img2, depth, counter - 3); depth = depth2; depth.filterNoise(); cout << timer.elapsed() << endl; depth.toInverseMat(res); depth.sigmaToMat(sigmaRes); // imwrite(imageDir + "res" + to_string(counter++) + ".png", depth*200); imshow("sigma " + to_string(counter), sigmaRes*20); imshow("d sigma " + to_string(counter), (sigmaRes - sigmaRes2)*20 + 0.5); // cout << (sigmaRes - sigmaRes2)(Rect(150, 150, 15, 15)) << endl; imshow("res " + to_string(counter), res *0.12); counter++; waitKey(); } waitKey(); return 0; }