int main(int argc, char *argv[])
{
reader pd("../config/config.ini");
camera cam;
cam.fx = atof(pd.get("camera.fx").c_str());
cam.fy = atof(pd.get("camera.fy").c_str());
cam.cx = atof(pd.get("camera.cx").c_str());
cam.cy = atof(pd.get("camera.cy").c_str());
cam.scale = atof(pd.get("camera.scale").c_str());
frame frame1, frame2;
frame1.rgb = imread("../data/rgb1.png");
frame1.depth = imread("../data/depth1.png", -1);
frame2.rgb = imread("../data/rgb2.png");
frame2.depth = imread("../data/depth2.png", -1);
cout << "extracting features" << endl;
string detector = pd.get("detector");
string descriptor = pd.get("descriptor");
compute_feature(frame1, detector, descriptor);
compute_feature(frame2, detector, descriptor);
cout << "matching" << endl;
vector<DMatch> matches;
match(matches, frame1, frame2);
cout << "solving pnp" << endl;
pnp result = sfm(cam, matches, frame1, frame2);
Isometry3d T = cvmat2eigen(result.r, result.t);
cout << "r: " << result.r << endl << "t: " << result.t << endl;
cout << "converting images into clouds" << endl;
Pointcloud::Ptr cloud1 = map2point(cam, frame1.rgb, frame1.depth);
cout << "combining clouds" << endl;
Pointcloud::Ptr output = joint(cam, cloud1, T, frame2);
io::savePCDFile("../data/result.pcd", *output);
cout << "result saved." << endl;
visualization::CloudViewer viewer("viewer");
viewer.showCloud(output);
while(!viewer.wasStopped()) {}
return 0;
}
void ImageFeature::process_binary(const unsigned char *input, int size, BinaryCollector *collector) {
int height, width;
unsigned char * image = image_bgr_fromstring(input, size, &height, &width);
int feature_size;
double* feature = compute_feature(image, height, width, &feature_size);
std::vector<double> vec(feature, feature + feature_size); // TODO: Remove copy!
std::vector<int> shape(1);
shape[0] = feature_size;
ndarray_tostring(vec, shape, collector);
delete [] feature;
delete [] image;
}
