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; }