Skip to content

Instantly share code, notes, and snippets.

@gunshi

gunshi/gist:2702c9677f30e936b532e508e45460ea

Created May 10, 2016 19:42
Show Gist options
  • Save gunshi/2702c9677f30e936b532e508e45460ea to your computer and use it in GitHub Desktop.
Save gunshi/2702c9677f30e936b532e508e45460ea to your computer and use it in GitHub Desktop.
Download ZIP
scene_reconstruction.cpp
Raw
gistfile1.txt
#define CERES_FOUND true
#include <opencv2/sfm.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <fstream>
#include <string>
using namespace std;
using namespace cv;
using namespace cv::sfm;
static void help() {
cout
<< "\n------------------------------------------------------------------------------------\n"
<< " This program shows the multiview reconstruction capabilities in the \n"
<< " OpenCV Structure From Motion (SFM) module.\n"
<< " It reconstruct a scene from a set of 2D images \n"
<< " Usage:\n"
<< " example_sfm_scene_reconstruction <path_to_file> <f> <cx> <cy>\n"
<< " where: path_to_file is the file absolute path into your system which contains\n"
<< " the list of images to use for reconstruction. \n"
<< " f is the focal lenght in pixels. \n"
<< " cx is the image principal point x coordinates in pixels. \n"
<< " cy is the image principal point y coordinates in pixels. \n"
<< "------------------------------------------------------------------------------------\n\n"
<< endl;
}
int getdir(const string _filename, std::vector<std::string> &files)
{
ifstream myfile(_filename.c_str());
if (!myfile.is_open()) {
cout << "Unable to read file: " << _filename << endl;
exit(0);
} else {;
size_t found = _filename.find_last_of("/\\");
string line_str, path_to_file = _filename.substr(0, found);
while ( getline(myfile, line_str) )
files.push_back(path_to_file+string("/")+line_str);
}
return 1;
}
int main(int argc, char* argv[])
{
// Read input parameters
argc=5;
argv[1]="dataset_files.txt";
argv[2]="350";
argv[3]="240";
argv[4]="360";
if ( argc != 5 )
{
help();
exit(0);
}
// Parse the image paths
std::vector<std::string> images_paths;
getdir( argv[1], images_paths );
// Build instrinsics
float f = atof(argv[2]),
cx = atof(argv[3]), cy = atof(argv[4]);
Matx33d K = Matx33d( f, 0, cx,
0, f, cy,
0, 0, 1);
/// Reconstruct the scene using the 2d images
bool is_projective = true;
std::vector<cv::Mat> Rs_est, ts_est, points3d_estimated;
reconstruct(images_paths, Rs_est, ts_est, K, points3d_estimated, is_projective);
// Print output
cout << "\n----------------------------\n" << endl;
cout << "Reconstruction: " << endl;
cout << "============================" << endl;
cout << "Estimated 3D points: " << points3d_estimated.size() << endl;
cout << "Estimated cameras: " << Rs_est.size() << endl;
cout << "Refined intrinsics: " << endl << K << endl << endl;
cout << "3D Visualization: " << endl;
cout << "============================" << endl;
/// Create 3D windows
viz::Viz3d window("Coordinate Frame");
window.setWindowSize(Size(500,500));
window.setWindowPosition(Point(150,150));
window.setBackgroundColor(); // black by default
// Create the pointcloud
cout << "Recovering points ... ";
// recover estimated points3d
vector<Vec3f> point_cloud_est; /////
for (int i = 0; i < points3d_estimated.size(); ++i)
point_cloud_est.push_back(Vec3f(points3d_estimated[i]));
cout << "[DONE]" << endl;
/// Recovering cameras
cout << "Recovering cameras ... ";
vector<Affine3d> path; /////
for (size_t i = 0; i < Rs_est.size(); ++i)
path.push_back(Affine3d(Rs_est[i],ts_est[i]));
cout << "[DONE]" << endl;
/// Add the pointcloud
if ( point_cloud_est.size() > 0 )
{
cout << "Rendering points ... ";
//
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.width = point_cloud_est.size();
cloud.height = 1;
cloud.is_dense = true;
cloud.points.resize (cloud.width * cloud.height);
for (size_t i = 0; i < cloud.points.size (); ++i)
{
cloud.points[i].x = point_cloud_est[i].Matx::val[0];
cloud.points[i].y = point_cloud_est[i].Matx::val[1];
cloud.points[i].z = point_cloud_est[i].Matx::val[2];
}
// pcl::io::savePCDFileASCII ("/Users/gunshigupta/Desktop/test_pcd.pcd", cloud);
std::cerr << "Saved " << cloud.points.size () << " data points to test_pcd.pcd." << std::endl;
for (size_t i = 0; i < cloud.points.size (); ++i)
std::cerr << " " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;
pcl::io::savePLYFileASCII ("/Users/gunshigupta/Desktop/test_pcd.ply", cloud);
// pcl::PointCloud<pcl::PointXYZ>::Ptr ptrcloud(&cloud);
//
// pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
// viewer.showCloud(ptrcloud);
// while (!viewer.wasStopped())
// {
// }
//
viz::WCloud cloud_widget(point_cloud_est, viz::Color::green());
window.showWidget("point_cloud", cloud_widget);
cout << "[DONE]" << endl;
}
else
{
cout << "Cannot render points: Empty pointcloud" << endl;
}
/// Add cameras
if ( path.size() > 0 )
{
cout << "Rendering Cameras ... ";
window.showWidget("cameras_frames_and_lines", viz::WTrajectory(path, viz::WTrajectory::BOTH, 0.1, viz::Color::green()));
window.showWidget("cameras_frustums", viz::WTrajectoryFrustums(path, K, 0.1, viz::Color::yellow()));
window.setViewerPose(path[0]);
cout << "[DONE]" << endl;
}
else
{
cout << "Cannot render the cameras: Empty path" << endl;
}
/// Wait for key 'q' to close the window
cout << endl << "Press 'q' to close each windows ... " << endl;
window.spin();
waitKey(0);
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment