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;
}
#include "core/or/extend_info_st.h"
#include "feature/rend/rend_authorized_client_st.h"
#include "feature/rend/rend_intro_point_st.h"
#include "feature/rend/rend_service_descriptor_st.h"
/** List of tokens recognized in rendezvous service descriptors */
static token_rule_t desc_token_table[] = {
T1_START("rendezvous-service-descriptor", R_RENDEZVOUS_SERVICE_DESCRIPTOR,
EQ(1), NO_OBJ),
T1("version", R_VERSION, EQ(1), NO_OBJ),
T1("permanent-key", R_PERMANENT_KEY, NO_ARGS, NEED_KEY_1024),
T1("secret-id-part", R_SECRET_ID_PART, EQ(1), NO_OBJ),
T1("publication-time", R_PUBLICATION_TIME, CONCAT_ARGS, NO_OBJ),
T1("protocol-versions", R_PROTOCOL_VERSIONS, EQ(1), NO_OBJ),
T01("introduction-points", R_INTRODUCTION_POINTS, NO_ARGS, NEED_OBJ),
T1_END("signature", R_SIGNATURE, NO_ARGS, NEED_OBJ),
END_OF_TABLE
};
/** List of tokens recognized in the (encrypted) list of introduction points of
* rendezvous service descriptors */
static token_rule_t ipo_token_table[] = {
T1_START("introduction-point", R_IPO_IDENTIFIER, EQ(1), NO_OBJ),
T1("ip-address", R_IPO_IP_ADDRESS, EQ(1), NO_OBJ),
T1("onion-port", R_IPO_ONION_PORT, EQ(1), NO_OBJ),
T1("onion-key", R_IPO_ONION_KEY, NO_ARGS, NEED_KEY_1024),
T1("service-key", R_IPO_SERVICE_KEY, NO_ARGS, NEED_KEY_1024),
END_OF_TABLE
};
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;
}
