int main( int argc, char* argv[] )
{
// Initialise window
pangolin::View& container = SetupPangoGLWithCuda(1024, 768);
size_t cu_mem_start, cu_mem_end, cu_mem_total;
cudaMemGetInfo( &cu_mem_start, &cu_mem_total );
glClearColor(1,1,1,0);
// Open video device
hal::Camera video = OpenRpgCamera(argc,argv);
// Capture first image
pb::ImageArray images;
// N cameras, each w*h in dimension, greyscale
const size_t N = video.NumChannels();
if( N != 2 ) {
std::cerr << "Two images are required to run this program!" << std::endl;
exit(1);
}
const size_t nw = video.Width();
const size_t nh = video.Height();
// Capture first image
video.Capture(images);
// Downsample this image to process less pixels
const int max_levels = 6;
const int level = roo::GetLevelFromMaxPixels( nw, nh, 640*480 );
// const int level = 4;
assert(level <= max_levels);
// Find centered image crop which aligns to 16 pixels at given level
const NppiRect roi = roo::GetCenteredAlignedRegion(nw,nh,16 << level,16 << level);
// Load Camera intrinsics from file
GetPot clArgs( argc, argv );
const std::string filename = clArgs.follow("","-cmod");
if( filename.empty() ) {
std::cerr << "Camera models file is required!" << std::endl;
exit(1);
}
const calibu::CameraRig rig = calibu::ReadXmlRig(filename);
if( rig.cameras.size() != 2 ) {
std::cerr << "Two camera models are required to run this program!" << std::endl;
exit(1);
}
Eigen::Matrix3f CamModel0 = rig.cameras[0].camera.K().cast<float>();
Eigen::Matrix3f CamModel1 = rig.cameras[1].camera.K().cast<float>();
roo::ImageIntrinsics camMod[] = {
{CamModel0(0,0),CamModel0(1,1),CamModel0(0,2),CamModel0(1,2)},
{CamModel1(0,0),CamModel1(1,1),CamModel1(0,2),CamModel1(1,2)}
};
for(int i=0; i<2; ++i ) {
// Adjust to match camera image dimensions
const double scale = nw / rig.cameras[i].camera.Width();
roo::ImageIntrinsics camModel = camMod[i].Scale( scale );
// Adjust to match cropped aligned image
camModel = camModel.CropToROI( roi );
camMod[i] = camModel;
}
const unsigned int w = roi.width;
const unsigned int h = roi.height;
const unsigned int lw = w >> level;
const unsigned int lh = h >> level;
const Eigen::Matrix3d& K0 = camMod[0].Matrix();
const Eigen::Matrix3d& Kl = camMod[0][level].Matrix();
std::cout << "K Matrix: " << std::endl << K0 << std::endl;
std::cout << "K Matrix - Level: " << std::endl << Kl << std::endl;
std::cout << "Video stream dimensions: " << nw << "x" << nh << std::endl;
std::cout << "Chosen Level: " << level << std::endl;
std::cout << "Processing dimensions: " << lw << "x" << lh << std::endl;
std::cout << "Offset: " << roi.x << "x" << roi.y << std::endl;
// print selected camera model
std::cout << "Camera Model used: " << std::endl << camMod[0][level].Matrix() << std::endl;
Eigen::Matrix3d RDFvision;RDFvision<< 1,0,0, 0,1,0, 0,0,1;
Eigen::Matrix3d RDFrobot; RDFrobot << 0,1,0, 0,0, 1, 1,0,0;
Eigen::Matrix4d T_vis_ro = Eigen::Matrix4d::Identity();
T_vis_ro.block<3,3>(0,0) = RDFvision.transpose() * RDFrobot;
Eigen::Matrix4d T_ro_vis = Eigen::Matrix4d::Identity();
T_ro_vis.block<3,3>(0,0) = RDFrobot.transpose() * RDFvision;
const Sophus::SE3d T_rl_orig = T_rlFromCamModelRDF(rig.cameras[0], rig.cameras[1], RDFvision);
// TODO(jmf): For now, assume cameras are rectified. Later allow unrectified cameras.
/*
double k1 = 0;
double k2 = 0;
if(cam[0].Type() == MVL_CAMERA_WARPED)
{
k1 = cam[0].GetModel()->warped.kappa1;
k2 = cam[0].GetModel()->warped.kappa2;
}
*/
const bool rectify = false;
if(!rectify) {
std::cout << "Using pre-rectified images" << std::endl;
}
// Check we received at least two images
if(images.Size() < 2) {
std::cerr << "Failed to capture first stereo pair from camera" << std::endl;
return -1;
}
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrixRDF_TopLeft(w,h,K0(0,0),K0(1,1),K0(0,2),K0(1,2),0.1,10000),
pangolin::IdentityMatrix(pangolin::GlModelViewStack)
);
pangolin::GlBufferCudaPtr vbo(pangolin::GlArrayBuffer, lw*lh,GL_FLOAT, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
pangolin::GlBufferCudaPtr cbo(pangolin::GlArrayBuffer, lw*lh,GL_UNSIGNED_BYTE, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
pangolin::GlBuffer ibo = pangolin::MakeTriangleStripIboForVbo(lw,lh);
// Allocate Camera Images on device for processing
roo::Image<unsigned char, roo::TargetHost, roo::DontManage> hCamImg[] = {{0,nw,nh},{0,nw,nh}};
roo::Image<float2, roo::TargetDevice, roo::Manage> dLookup[] = {{w,h},{w,h}};
roo::Image<unsigned char, roo::TargetDevice, roo::Manage> upload(w,h);
roo::Pyramid<unsigned char, max_levels, roo::TargetDevice, roo::Manage> img_pyr[] = {{w,h},{w,h}};
roo::Image<float, roo::TargetDevice, roo::Manage> img[] = {{lw,lh},{lw,lh}};
roo::Volume<float, roo::TargetDevice, roo::Manage> vol[] = {{lw,lh,MAXD},{lw,lh,MAXD}};
roo::Image<float, roo::TargetDevice, roo::Manage> disp[] = {{lw,lh},{lw,lh}};
roo::Image<float, roo::TargetDevice, roo::Manage> meanI(lw,lh);
roo::Image<float, roo::TargetDevice, roo::Manage> varI(lw,lh);
roo::Image<float, roo::TargetDevice, roo::Manage> temp[] = {{lw,lh},{lw,lh},{lw,lh},{lw,lh},{lw,lh}};
roo::Image<float,roo::TargetDevice, roo::Manage>& imgd = disp[0];
roo::Image<float,roo::TargetDevice, roo::Manage> depthmap(lw,lh);
roo::Image<float,roo::TargetDevice, roo::Manage> imga(lw,lh);
roo::Image<float2,roo::TargetDevice, roo::Manage> imgq(lw,lh);
roo::Image<float,roo::TargetDevice, roo::Manage> imgw(lw,lh);
roo::Image<float4, roo::TargetDevice, roo::Manage> d3d(lw,lh);
roo::Image<unsigned char, roo::TargetDevice,roo::Manage> Scratch(lw*sizeof(roo::LeastSquaresSystem<float,6>),lh);
typedef ulong4 census_t;
roo::Image<census_t, roo::TargetDevice, roo::Manage> census[] = {{lw,lh},{lw,lh}};
// Stereo transformation (post-rectification)
Sophus::SE3d T_rl = T_rl_orig;
const double baseline = T_rl.translation().norm();
std::cout << "Baseline: " << baseline << std::endl;
cudaMemGetInfo( &cu_mem_end, &cu_mem_total );
std::cout << "CuTotal: " << cu_mem_total/(1024*1024) << ", Available: " << cu_mem_end/(1024*1024) << ", Used: " << (cu_mem_start-cu_mem_end)/(1024*1024) << std::endl;
pangolin::Var<bool> step("ui.step", false, false);
pangolin::Var<bool> run("ui.run", false, true);
pangolin::Var<bool> lockToCam("ui.Lock to cam", false, true);
pangolin::Var<int> show_slice("ui.show slice",MAXD/2, 0, MAXD-1);
pangolin::Var<int> maxdisp("ui.maxdisp",MAXD, 0, MAXD);
pangolin::Var<bool> subpix("ui.subpix", true, true);
pangolin::Var<bool> use_census("ui.use census", true, true);
pangolin::Var<int> avg_rad("ui.avg_rad",0, 0, 100);
pangolin::Var<bool> do_dtam("ui.do dtam", false, true);
pangolin::Var<bool> dtam_reset("ui.reset", false, false);
pangolin::Var<float> g_alpha("ui.g alpha", 14, 0,4);
pangolin::Var<float> g_beta("ui.g beta", 2.5, 0,2);
pangolin::Var<float> theta("ui.theta", 100, 0,100);
pangolin::Var<float> lambda("ui.lambda", 20, 0,20);
pangolin::Var<float> sigma_q("ui.sigma q", 0.7, 0, 1);
pangolin::Var<float> sigma_d("ui.sigma d", 0.7, 0, 1);
pangolin::Var<float> huber_alpha("ui.huber alpha", 0.002, 0, 0.01);
pangolin::Var<float> beta("ui.beta", 0.00001, 0, 0.01);
pangolin::Var<float> alpha("ui.alpha", 0.9, 0,1);
pangolin::Var<float> r1("ui.r1", 100, 0,0.01);
pangolin::Var<float> r2("ui.r2", 100, 0,0.01);
pangolin::Var<bool> filter("ui.filter", false, true);
pangolin::Var<float> eps("ui.eps",0.01*0.01, 0, 0.01);
pangolin::Var<int> rad("ui.radius",9, 1, 20);
pangolin::Var<bool> leftrightcheck("ui.left-right check", false, true);
pangolin::Var<float> maxdispdiff("ui.maxdispdiff",1, 0, 5);
pangolin::Var<int> domedits("ui.median its",1, 1, 10);
pangolin::Var<bool> domed9x9("ui.median 9x9", false, true);
pangolin::Var<bool> domed7x7("ui.median 7x7", false, true);
pangolin::Var<bool> domed5x5("ui.median 5x5", false, true);
pangolin::Var<int> medi("ui.medi",12, 0, 24);
pangolin::Var<float> filtgradthresh("ui.filt grad thresh", 0, 0, 20);
pangolin::Var<bool> save_depthmaps("ui.save_depthmaps", false, true);
int jump_frames = 0;
pangolin::RegisterKeyPressCallback(' ', [&run](){run = !run;} );
pangolin::RegisterKeyPressCallback('l', [&lockToCam](){lockToCam = !lockToCam;} );
pangolin::RegisterKeyPressCallback(pangolin::PANGO_SPECIAL + GLUT_KEY_RIGHT, [&step](){step=true;} );
pangolin::RegisterKeyPressCallback(']', [&jump_frames](){jump_frames=100;} );
pangolin::RegisterKeyPressCallback('}', [&jump_frames](){jump_frames=1000;} );
pangolin::Handler2dImageSelect handler2d(lw,lh,level);
// ActivateDrawPyramid<unsigned char,max_levels> adleft(img_pyr[0],GL_LUMINANCE8, false, true);
// ActivateDrawPyramid<unsigned char,max_levels> adright(img_pyr[1],GL_LUMINANCE8, false, true);
pangolin::ActivateDrawImage<float> adleft(img[0],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adright(img[1],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adisp(disp[0],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adw(imgw,GL_LUMINANCE32F_ARB, false, true);
// ActivateDrawImage<float> adCrossSection(dCrossSection,GL_RGBA_FLOAT32_APPLE, false, true);
pangolin::ActivateDrawImage<float> adVol(vol[0].ImageXY(show_slice),GL_LUMINANCE32F_ARB, false, true);
SceneGraph::GLSceneGraph graph;
SceneGraph::GLVbo glvbo(&vbo,&ibo,&cbo);
graph.AddChild(&glvbo);
SetupContainer(container, 6, (float)w/h);
container[0].SetDrawFunction(boost::ref(adleft)).SetHandler(&handler2d);
container[1].SetDrawFunction(boost::ref(adright)).SetHandler(&handler2d);
container[2].SetDrawFunction(boost::ref(adisp)).SetHandler(&handler2d);
container[3].SetDrawFunction(boost::ref(adVol)).SetHandler(&handler2d);
container[4].SetDrawFunction(SceneGraph::ActivateDrawFunctor(graph, s_cam))
.SetHandler( new pangolin::Handler3D(s_cam, pangolin::AxisNone) );
container[5].SetDrawFunction(boost::ref(adw)).SetHandler(&handler2d);
for(unsigned long frame=0; !pangolin::ShouldQuit();)
{
bool go = frame==0 || jump_frames > 0 || run || Pushed(step);
for(; jump_frames > 0; jump_frames--) {
video.Capture(images);
}
if(go) {
if(frame>0) {
if( video.Capture(images) == false) {
exit(1);
}
}
frame++;
/////////////////////////////////////////////////////////////
// Upload images to device (Warp / Decimate if necessery)
for(int i=0; i<2; ++i ) {
hCamImg[i].ptr = images[i].data();
if(rectify) {
upload.CopyFrom(hCamImg[i].SubImage(roi));
Warp(img_pyr[i][0], upload, dLookup[i]);
}else{
img_pyr[i][0].CopyFrom(hCamImg[i].SubImage(roi));
}
roo::BoxReduce<unsigned char, max_levels, unsigned int>(img_pyr[i]);
}
}
go |= avg_rad.GuiChanged() | use_census.GuiChanged();
if( go ) {
for(int i=0; i<2; ++i ) {
roo::ElementwiseScaleBias<float,unsigned char,float>(img[i], img_pyr[i][level],1.0f/255.0f);
if(avg_rad > 0 ) {
roo::BoxFilter<float,float,float>(temp[0],img[i],Scratch,avg_rad);
roo::ElementwiseAdd<float,float,float,float>(img[i], img[i], temp[0], 1, -1, 0.5);
}
if(use_census) {
Census(census[i], img[i]);
}
}
}
if( go | g_alpha.GuiChanged() || g_beta.GuiChanged() ) {
ExponentialEdgeWeight(imgw, img[0], g_alpha, g_beta);
}
go |= filter.GuiChanged() | leftrightcheck.GuiChanged() | rad.GuiChanged() | eps.GuiChanged() | alpha.GuiChanged() | r1.GuiChanged() | r2.GuiChanged();
if(go) {
if(use_census) {
roo::CensusStereoVolume<float, census_t>(vol[0], census[0], census[1], maxdisp, -1);
if(leftrightcheck) roo::CensusStereoVolume<float, census_t>(vol[1], census[1], census[0], maxdisp, +1);
}else{
CostVolumeFromStereoTruncatedAbsAndGrad(vol[0], img[0], img[1], -1, alpha, r1, r2);
if(leftrightcheck) CostVolumeFromStereoTruncatedAbsAndGrad(vol[1], img[1], img[0], +1, alpha, r1, r2);
}
if(filter) {
// Filter Cost volume
for(int v=0; v<(leftrightcheck?2:1); ++v)
{
roo::Image<float, roo::TargetDevice, roo::Manage>& I = img[v];
roo::ComputeMeanVarience<float,float,float>(varI, temp[0], meanI, I, Scratch, rad);
for(int d=0; d<maxdisp; ++d)
{
roo::Image<float> P = vol[v].ImageXY(d);
roo::ComputeCovariance(temp[0],temp[2],temp[1],P,meanI,I,Scratch,rad);
GuidedFilter(P,temp[0],varI,temp[1],meanI,I,Scratch,temp[2],temp[3],temp[4],rad,eps);
}
}
}
}
static int n = 0;
// static float theta = 0;
// go |= Pushed(dtam_reset);
// if(go )
if(Pushed(dtam_reset))
{
n = 0;
theta.Reset();
// Initialise primal and auxillary variables
CostVolMinimumSubpix(imgd,vol[0], maxdisp,-1);
imga.CopyFrom(imgd);
// Initialise dual variable
imgq.Memset(0);
}
const double min_theta = 1E-0;
if(do_dtam && theta > min_theta)
{
for(int i=0; i<5; ++i ) {
// Auxillary exhaustive search
CostVolMinimumSquarePenaltySubpix(imga, vol[0], imgd, maxdisp, -1, lambda, (theta) );
// Dual Ascent
roo::WeightedHuberGradU_DualAscentP(imgq, imgd, imgw, sigma_q, huber_alpha);
// Primal Descent
roo::WeightedL2_u_minus_g_PrimalDescent(imgd, imgq, imga, imgw, sigma_d, 1.0f / (theta) );
theta= theta * (1-beta*n);
++n;
}
if( theta <= min_theta && save_depthmaps ) {
cv::Mat dmap = cv::Mat( lh, lw, CV_32FC1 );
// convert disparity to depth
roo::Disp2Depth(imgd, depthmap, Kl(0,0), baseline );
depthmap.MemcpyToHost( dmap.data );
// save depth image
char Index[10];
sprintf( Index, "%05d", frame );
std::string DepthPrefix = "SDepth-";
std::string DepthFile;
DepthFile = DepthPrefix + Index + ".pdm";
std::cout << "Depth File: " << DepthFile << std::endl;
std::ofstream pDFile( DepthFile.c_str(), std::ios::out | std::ios::binary );
pDFile << "P7" << std::endl;
pDFile << dmap.cols << " " << dmap.rows << std::endl;
unsigned int Size = dmap.elemSize1() * dmap.rows * dmap.cols;
pDFile << 4294967295 << std::endl;
pDFile.write( (const char*)dmap.data, Size );
pDFile.close();
// save grey image
std::string GreyPrefix = "Left-";
std::string GreyFile;
GreyFile = GreyPrefix + Index + ".pgm";
std::cout << "Grey File: " << GreyFile << std::endl;
cv::Mat gimg = cv::Mat( lh, lw, CV_8UC1 );
img_pyr[0][level].MemcpyToHost( gimg.data );
cv::imwrite( GreyFile, gimg );
// reset
step = true;
dtam_reset = true;
}
}
go |= pangolin::GuiVarHasChanged();
// if(go) {
// if(subpix) {
// CostVolMinimumSubpix(disp[0],vol[0], maxdisp,-1);
// if(leftrightcheck) CostVolMinimumSubpix(disp[1],vol[1], maxdisp,+1);
// }else{
// CostVolMinimum<float,float>(disp[0],vol[0], maxdisp);
// if(leftrightcheck) CostVolMinimum<float,float>(disp[1],vol[1], maxdisp);
// }
// }
if(go) {
for(int di=0; di<(leftrightcheck?2:1); ++di) {
for(int i=0; i < domedits; ++i ) {
if(domed9x9) MedianFilterRejectNegative9x9(disp[di],disp[di], medi);
if(domed7x7) MedianFilterRejectNegative7x7(disp[di],disp[di], medi);
if(domed5x5) MedianFilterRejectNegative5x5(disp[di],disp[di], medi);
}
}
if(leftrightcheck ) {
LeftRightCheck(disp[1], disp[0], +1, maxdispdiff);
LeftRightCheck(disp[0], disp[1], -1, maxdispdiff);
}
if(filtgradthresh > 0) {
FilterDispGrad(disp[0], disp[0], filtgradthresh);
}
}
// if(go)
{
// Generate point cloud from disparity image
DisparityImageToVbo(d3d, disp[0], baseline, Kl(0,0), Kl(1,1), Kl(0,2), Kl(1,2) );
// if(container[3].IsShown())
{
// Copy point cloud into VBO
{
pangolin::CudaScopedMappedPtr var(vbo);
roo::Image<float4> dVbo((float4*)*var,lw,lh);
dVbo.CopyFrom(d3d);
}
// Generate CBO
{
pangolin::CudaScopedMappedPtr var(cbo);
roo::Image<uchar4> dCbo((uchar4*)*var,lw,lh);
roo::ConvertImage<uchar4,unsigned char>(dCbo, img_pyr[0][level]);
}
}
// Update texture views
adisp.SetImageScale(1.0f/maxdisp);
// adleft.SetLevel(show_level);
// adright.SetLevel(show_level);
adVol.SetImage(vol[0].ImageXY(show_slice));
}
/////////////////////////////////////////////////////////////
// Draw
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f(1,1,1);
pangolin::FinishFrame();
}
}
int main( int argc, char* argv[] )
{
const int w = 1024;
const int h = 768;
// Initialise window
View& container = SetupPangoGLWithCuda(w, h);
ApplyPreferredGlSettings();
// Open video device
const roo::ImageIntrinsics K( 570.342, 570.342, w/2.0 - 0.5, h/2.0 - 0.5 );
roo::Image<float, roo::TargetDevice, roo::Manage> ray_i(w,h);
roo::Image<float, roo::TargetDevice, roo::Manage> ray_d(w,h);
roo::Image<float, roo::TargetDevice, roo::Manage> ray_dist(w,h);
roo::Image<float4, roo::TargetDevice, roo::Manage> ray_n(w,h);
roo::Image<float4, roo::TargetDevice, roo::Manage> vis(w,h);
roo::BoundedVolume<roo::SDF_t, roo::TargetDevice, roo::Manage> vol;
roo::BoundedVolume<roo::SDF_t, roo::TargetDevice, roo::Manage> vol2;
LoadPXM("save.vol", vol);
LoadPXM("save2.vol", vol2);
SceneGraph::GLSceneGraph glgraph;
SceneGraph::GLAxisAlignedBox glboxvol;
glboxvol.SetBounds(roo::ToEigen(vol.bbox.Min()), roo::ToEigen(vol.bbox.Max()) );
glgraph.AddChild(&glboxvol);
pangolin::OpenGlRenderState s_cam(
ProjectionMatrixRDF_TopLeft(w,h,K.fu,K.fv,K.u0,K.v0,0.1,1000),
ModelViewLookAtRDF(0,0,-2,0,0,0,0,-1,0)
);
Var<float> trunc_dist_factor("ui.trunc_vol_factor",2, 1, 4);
Var<bool> switch_sdf("ui.switch_sdf",false,true);
Var<bool> diff_sdf("ui.diff_sdf",true,true);
ActivateDrawImage<float4> adrayimg(vis, GL_RGBA32F, true, true);
Handler3DGpuDepth rayhandler(ray_d, s_cam, AxisNone);
SetupContainer(container, 2, (float)w/h);
container[0].SetDrawFunction(boost::ref(adrayimg))
.SetHandler(&rayhandler);
container[1].SetDrawFunction(SceneGraph::ActivateDrawFunctor(glgraph, s_cam))
.SetHandler( new Handler3D(s_cam, AxisNone) );
while(!pangolin::ShouldQuit())
{
const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());
Sophus::SE3d T_vw(s_cam.GetModelViewMatrix());
const roo::BoundingBox roi(T_vw.inverse().matrix3x4(), w, h, K, 0, 50);
roo::BoundedVolume<roo::SDF_t> work_vol = (switch_sdf ? vol2 : vol).SubBoundingVolume( roi );
roo::BoundedVolume<roo::SDF_t> work_vol2 = (switch_sdf ? vol : vol2).SubBoundingVolume( roi );
if(work_vol.IsValid()) {
roo::RaycastSdf(ray_d, ray_n, ray_i, work_vol, T_vw.inverse().matrix3x4(), K, 0.1, 50, trunc_dist, true );
roo::SdfDistance(ray_dist, ray_d, work_vol2, T_vw.inverse().matrix3x4(), K, trunc_dist);
if(!diff_sdf) roo::Fill<float>(ray_dist, 0.0f);
roo::Remap(vis, ray_i, ray_dist, -trunc_dist, trunc_dist);
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f(1,1,1);
pangolin::FinishFrame();
}
}
int main( int argc, char* argv[] )
{
// Create OpenGL window in single line thanks to GLUT
pangolin::CreateWindowAndBind("Main",640,480);
SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
glewInit();
// Scenegraph to hold GLObjects and relative transformations
SceneGraph::GLSceneGraph glGraph;
SceneGraph::GLLight light(10,10,-100);
glGraph.AddChild(&light);
// Define grid object
SceneGraph::GLGrid glGrid(50,2.0, true);
// Define axis object, and set its pose
SceneGraph::GLAxis glAxis;
glAxis.SetPose(-1,-2,-0.1, 0, 0, M_PI/4);
glAxis.SetScale(0.25);
SceneGraph::GLMovableAxis glMovableAxis;
glMovableAxis.SetPosition(-3,3,-1);
SceneGraph::GLAxisAlignedBox glBox;
glBox.SetResizable();
// Define movable waypoint object with velocity
SceneGraph::GLWayPoint glWaypoint;
glWaypoint.SetPose(0.5,0.5,-0.1,0,0,0);
// Optionally clamp waypoint to specific plane
glWaypoint.ClampToPlane(Eigen::Vector4d(0,0,1,0));
// Define 3D spiral using a GLCachedPrimitives object
SceneGraph::GLCachedPrimitives glSpiral(GL_LINE_STRIP, SceneGraph::GLColor(1.0f,0.7f,0.2f));
for(double t=0; t < 10*M_PI; t+= M_PI/50) {
glSpiral.AddVertex(Eigen::Vector3d(cos(t)+2, sin(t)+2, -0.1*t) );
}
// Define 3D floating text object
SceneGraph::GLText glText3d("3D Floating Text", -1, 1, -1);
// Add objects to scenegraph
glGraph.AddChild(&glGrid);
glGraph.AddChild(&glWaypoint);
glGraph.AddChild(&glSpiral);
glGraph.AddChild(&glAxis);
glGraph.AddChild(&glText3d);
glGraph.AddChild(&glMovableAxis);
glMovableAxis.AddChild(&glBox);
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState stacks3d(
pangolin::ProjectionMatrix(640,480,420,420,320,240,0.1,1000),
pangolin::ModelViewLookAt(0,-2,-4, 0,1,0, pangolin::AxisNegZ)
);
// Pangolin abstracts the OpenGL viewport as a View.
// Here we get a reference to the default 'base' view.
pangolin::View& container = pangolin::DisplayBase();
// We define a new view which will reside within the container.
pangolin::View view3d;
// We set the views location on screen and add a handler which will
// let user input update the model_view matrix (stacks3d) and feed through
// to our scenegraph
view3d.SetBounds(0.0, 1.0, 0.0, 1.0, 640.0f/480.0f)
.SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d,pangolin::AxisNegZ))
.SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));
// Add our views as children to the base container.
container.AddDisplay(view3d);
GLWidgetView panel;
panel.Init(0,1,0,Attach::Pix(200),&WidgetDrawFunction);
container.AddDisplay(panel);
// Demonstration of how we can register a keyboard hook to trigger a method
pangolin::RegisterKeyPressCallback( pangolin::PANGO_CTRL + 'r', std::bind(GlobalKeyHook, "You Pushed ctrl-r!" ) );
// Add keyhook to save window contents (including alpha). The framebuffer is saved just before it is swapped
pangolin::RegisterKeyPressCallback( 's', std::bind(&pangolin::View::SaveOnRender, &pangolin::DisplayBase(), "window_OnRender" ) );
// Add keyhook to save a particular view (including alpha) at 4 times the resolution of the screen. This creates an FBO and renders into it straight away.
pangolin::RegisterKeyPressCallback( 'r', std::bind(&pangolin::View::SaveRenderNow, &view3d, "view3d_RenderNow", 4 ) );
// Default hooks for exiting (Esc) and fullscreen (tab).
while( !pangolin::ShouldQuit() )
{
// Clear whole screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Swap frames and Process Events
pangolin::FinishFrame();
// Pause for 1/60th of a second.
std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 60));
}
return 0;
}
int main( int argc, char* argv[] )
{
if(argc != 2) {
Usage();
exit(-1);
}
const std::string model_filename(argv[1]);
// Create OpenGL window in single line thanks to GLUT
pangolin::CreateWindowAndBind("Main",640,480);
SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
glClearColor( 0,0,0,0);
glewInit();
// Scenegraph to hold GLObjects and relative transformations
SceneGraph::GLSceneGraph glGraph;
SceneGraph::GLLight light(10,10,-100);
glGraph.AddChild(&light);
SceneGraph::GLGrid grid(10,1,true);
glGraph.AddChild(&grid);
SceneGraph::AxisAlignedBoundingBox bbox;
#ifdef HAVE_ASSIMP
// Define a mesh object and try to load model
SceneGraph::GLMesh glMesh;
try {
glMesh.Init(model_filename);
glGraph.AddChild(&glMesh);
bbox = glMesh.ObjectAndChildrenBounds();
}catch(exception e) {
cerr << "Cannot load mesh." << endl;
cerr << e.what() << std::endl;
exit(-1);
}
#endif // HAVE_ASSIMP
const Eigen::Vector3d center = bbox.Center();
double size = bbox.Size().norm();
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState stacks3d(
pangolin::ProjectionMatrix(640,480,420,420,320,240, 0.01, 1000),
pangolin::ModelViewLookAt(center(0), center(1) + size, center(2) + size/4, center(0), center(1), center(2), pangolin::AxisZ)
);
// We define a new view which will reside within the container.
pangolin::View view3d;
// We set the views location on screen and add a handler which will
// let user input update the model_view matrix (stacks3d) and feed through
// to our scenegraph
view3d.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
.SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d))
.SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));
// Add our views as children to the base container.
pangolin::DisplayBase().AddDisplay(view3d);
// Default hooks for exiting (Esc) and fullscreen (tab).
while( !pangolin::ShouldQuit() )
{
// Clear whole screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Swap frames and Process Events
pangolin::FinishFrame();
// Pause for 1/60th of a second.
std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 60));
}
return 0;
}
int main( int argc, char* argv[] )
{
if(argc != 2) {
Usage();
exit(-1);
}
std::string obj_basename(argv[1]);
std::size_t find_dot = obj_basename.find(".obj");
std::size_t find_slash = obj_basename.find_last_of('/');
std::cout<<" find_dot = " << find_dot << std::endl;
std::cout<<"find_slash = " << find_slash << std::endl;
obj_basename = obj_basename.substr(find_slash+1,find_dot-find_slash-1);
std::string data_dir = "../data/" + obj_basename + "_data";
const std::string model_filename(argv[1]);
int UI_WIDTH = 150;
// Create OpenGL window in single line thanks to GLUT
pangolin::CreateWindowAndBind("Main",640+UI_WIDTH,480);
SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
glClearColor( 0,0,0,0);
glewInit();
std::string system_command = std::string("grep -R \"o \" ") + std::string(argv[1]) +
std::string(" > object_names.txt");
std::cout<<system_command << std::endl;
system(system_command.c_str());
std::vector<std::string>objectNames;
ifstream objectNamesFile("object_names.txt");
if ( objectNamesFile.is_open())
{
char readlinedata[200];
while(1)
{
objectNamesFile.getline(readlinedata,200);
if ( objectNamesFile.eof())
break;
istringstream iss(readlinedata);
std::string objectName(iss.str());
objectName = objectName.substr(2,objectName.size()-2);
std::cout<<objectName<< std::endl;
objectNames.push_back(objectName);
}
}
objectNamesFile.close();
// Count the number of shapes
int max_label = 0;
for (int i = 0; i < objectNames.size(); i++)
{
int training_label = obj_label2training_label(objectNames.at(i));
max_label = std::max(max_label, training_label);
}
Eigen::MatrixXd colours(max_label,3);
std::map<int, int>colour2indexMap;
// <= since the label was seen, so need to include
for ( int i = 0; i <= max_label; i++)
{
// With the same seeded label, the colors should be the same across runs
// +1 since srand(0) is srand(1) since rand can't be initialized by 0
srand(i+1);
colours(i, 0) = static_cast<float>(rand()) /
static_cast<float>(RAND_MAX);
colours(i, 1) = static_cast<float>(rand()) /
static_cast<float>(RAND_MAX);
colours(i, 2) = static_cast<float>(rand()) /
static_cast<float>(RAND_MAX);
colour2indexMap[ (int)round((colours(i,0))*255) +
((int)round(colours(i,1)*255))*256 +
((int)round(colours(i,2)*255))*256*256] = i;
}
Eigen::MatrixXd renderingColours(objectNames.size(),3);
for(int i = 0; i < objectNames.size();i++)
{
int training_label = obj_label2training_label(objectNames.at(i));
renderingColours(i,0) = colours(training_label,0);
renderingColours(i,1) = colours(training_label,1);
renderingColours(i,2) = colours(training_label,2);
}
// Scenegraph to hold GLObjects and relative transformations
SceneGraph::GLSceneGraph glGraph;
SceneGraph::GLLight light(10,10,-100);
glGraph.AddChild(&light);
SceneGraph::AxisAlignedBoundingBox bbox;
#ifdef HAVE_ASSIMP
// Define a mesh object and try to load model
SceneGraph::GLMesh glMesh;
try {
glMesh.Init(model_filename);
glGraph.AddChild(&glMesh);
bbox = glMesh.ObjectAndChildrenBounds();
}catch(exception e) {
cerr << "Cannot load mesh." << endl;
cerr << e.what() << std::endl;
exit(-1);
}
#endif // HAVE_ASSIMP
const Eigen::Vector3d center = bbox.Center();
double size = bbox.Size().norm();
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState stacks3d(
pangolin::ProjectionMatrixRDF_BottomLeft(640,480,420,420,320,240, 0.1, 1000),
pangolin::ModelViewLookAt(center(0), center(1) + size, center(2) + size/4,
center(0), center(1), center(2), pangolin::AxisNegZ)
);
/// Create a Panel
pangolin::View& d_panel = pangolin::CreatePanel("ui")
.SetBounds(1.0, 0.0, 0, pangolin::Attach::Pix(UI_WIDTH));
/// Add named OpenGL viewport to window and provide 3D Handler
pangolin::View& d_cam = pangolin::Display("cam")
.SetBounds(0.0, 1, Attach::Pix(UI_WIDTH), 1, -640.0f/480.0f)
.SetHandler(new Handler3D(stacks3d));
int width = 640;
int height = 480;
std::vector<TooN::SE3<> >poses2render;
int render_pose_count = 0;
float depth_arrayf[width*height];
CVD::Image<u_int16_t>depth_image(CVD::ImageRef(width,height));
float near =0.1;
float far = 1000;
srand (time(NULL));
CVD::Image<CVD::Rgb<CVD::byte> > img_flipped(CVD::ImageRef(640,480));
char trajectory_fileName[300];
// bedroom1_poses_0_interpolated.txt
std::cout<<"data_dir = " << data_dir << std::endl;
std::cout<<"obj_basename = " << obj_basename << std::endl;
sprintf(trajectory_fileName,"%s/%s_trajectory.txt",
data_dir.c_str(),
obj_basename.c_str());
std::cout<<"Trajectory_fileName = " << trajectory_fileName << std::endl;
ifstream SE3PoseFile(trajectory_fileName);
TooN::SE3<>T_wc;
if (SE3PoseFile.is_open())
{
while(1)
{
SE3PoseFile >> T_wc;
if ( SE3PoseFile.eof() )
break;
poses2render.push_back(T_wc);
}
}
SE3PoseFile.close();
OpenGlMatrix openglSE3Matrix;
int skip_frame = 1;
ofstream model_file("3dmodel.obj");
float u0 = 320.0;
float v0 = 240.0;
float fx = 420.0;
float fy =-420.0;
// Default hooks for exiting (Esc) and fullscreen (tab).
while( !pangolin::ShouldQuit() )
{
static Var<int>numposes2plot("ui.numposes2plot",0,0,100);
{
numposes2plot = render_pose_count;
if ( numposes2plot >= (int)poses2render.size())
return 1;
TooN::SE3<>T_wc = poses2render.at(render_pose_count);
TooN::SE3<>T_cw = T_wc.inverse();
TooN::SO3<>Rot = T_cw.get_rotation();
TooN::Matrix<4>SE3Mat = TooN::Identity(4);
/// copy rotation
TooN::Matrix<3>SO3Mat = Rot.get_matrix();
SE3Mat.slice(0,0,3,3) = SO3Mat;
/// copy translation
TooN::Vector<3>trans = T_cw.get_translation();
SE3Mat(0,3) = trans[0];
SE3Mat(1,3) = trans[1];
SE3Mat(2,3) = trans[2];
/// Ref: http://www.felixgers.de/teaching/jogl/generalTransfo.html
/// It should be a transpose - stored in column major
for(int col = 0; col < 4; col++ )
{
for(int row = 0; row < 4; row++)
{
openglSE3Matrix.m[col*4+row] = SE3Mat(row,col);
}
}
/// set the model view matrix to this pose
stacks3d.SetModelViewMatrix(openglSE3Matrix);
stacks3d.Apply();
numposes2plot = render_pose_count;
// Clear whole screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// d_cam.ActivateAndScissor();
d_cam.ActivateScissorAndClear(stacks3d);
glEnable(GL_DEPTH_TEST);
glClear(GL_COLOR_BUFFER_BIT);
#ifdef GENERATE_RGBD_VIDEO
glMesh.DrawCanonicalObject();
#else
glMesh.DrawCanonicalObjectSegmentation(renderingColours);
#endif
CVD::Image<CVD::Rgb<CVD::byte> > img = CVD::glReadPixels<CVD::Rgb<CVD::byte> >(CVD::ImageRef(640,480),
CVD::ImageRef(150,0));
/// save the image
//#pragma omp parallel for
for(int yy = 0; yy < height; yy++ )
{
for(int xx = 0; xx < width; xx++)
{
img_flipped[CVD::ImageRef(xx,height-1-yy)] = img[CVD::ImageRef(xx,yy)];
}
}
char fileName[300];
sprintf(fileName,"%s/scene_00_%07d.png",data_dir.c_str(),render_pose_count/skip_frame);
CVD::img_save(img_flipped,fileName);
// CVD::Image<CVD::byte>labelImage = CVD::Image<CVD::byte>(CVD::ImageRef(640,480));
// /// save the annotations
//#pragma omp parallel for
// for(int yy = 0; yy < height; yy++ )
// {
// for(int xx = 0; xx < width; xx++)
// {
// CVD::Rgb<CVD::byte> pix = img_flipped[CVD::ImageRef(xx,yy)];
// int ind = pix.red + 256*pix.green + 256*256*pix.blue;
// labelImage[CVD::ImageRef(xx,yy)] = colour2indexMap[ind];
// }
// }
// sprintf(fileName,"%s/label_00_%07d.png",data_dir.c_str(),render_pose_count/skip_frame);
// CVD::img_save(labelImage,fileName);
glReadPixels(150, 0, 640, 480, GL_DEPTH_COMPONENT, GL_FLOAT, depth_arrayf);
int scale = 5000;
/// convert to real-depth
//#pragma omp parallel for
for(int i = 0; i < width*height; ++i)
{
float z_b = depth_arrayf[i];
float z_n = 2.0f * z_b - 1.0f;
depth_arrayf[i] = 2.0 * near * far / (far + near - z_n * (far - near));
}
/// save the depth image
//#pragma omp parallel for
for(int y = 0; y < height; y++)
{
for(int x = 0; x < width; x++)
{
int ind = (height-1-y)*width+x;
float depth_val = depth_arrayf[ind];
u_int16_t val = (u_int16_t)(depth_val * scale);
if( val < 65535 && depth_val < 100)
depth_image[CVD::ImageRef(x,y)] = val;
else
depth_image[CVD::ImageRef(x,y)] = 0 ;
}
}
char depthImageFileName[300];
{
sprintf(depthImageFileName,"%s/scenedepth_00_%07d.png",data_dir.c_str(),render_pose_count/skip_frame);
}
float rand_r = (float)rand()/RAND_MAX;
float rand_g = (float)rand()/RAND_MAX;
float rand_b = (float)rand()/RAND_MAX;
if( render_pose_count%1000 == 0 )
{
float max_depth = *std::max_element(depth_arrayf,depth_arrayf+width*height);
float min_depth = *std::min_element(depth_arrayf,depth_arrayf+width*height);
std::cout<<"max_depth = " << max_depth << std::endl;
std::cout<<"min_depth = " << min_depth << std::endl;
for(int y = 0; y < height; y++)
{
for(int x = 0; x < width; x++)
{
float depth = (float)depth_image[CVD::ImageRef(x,y)]/scale;
if ( depth < 10 && depth > 0 )
{
TooN::Vector<4>p_w = T_wc * TooN::makeVector(depth*(x-u0)/fx,
depth*(y-v0)/fy,
depth,
1.0);
model_file << "v " << p_w[0]<<" "<<p_w[1]<<" "<<p_w[2]<<" "<<rand_r<<" "<<rand_g<<" "<<rand_b<<std::endl;
}
}
}
}
std::string depthImageFileName_string(depthImageFileName);
CVD::img_save(depth_image,depthImageFileName_string);
render_pose_count++;
d_panel.Render();
// Swap frames and Process Events
pangolin::FinishFrame();
}
}
return 0;
}
int main( int /*argc*/, char** /*argv[]*/ )
{
// Create OpenGL window in single line thanks to GLUT
pangolin::CreateGlutWindowAndBind("Main",640,480, GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_MULTISAMPLE);
SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
glewInit();
// Scenegraph to hold GLObjects and relative transformations
SceneGraph::GLSceneGraph glGraph;
// Create shadow light to illuminate scene. Shadow casters
// and receivers must be set up individually.
SceneGraph::GLShadowLight shadowLight(10,10,-100, 2048,2048);
glGraph.AddChild(&shadowLight);
// Define grid object
SceneGraph::GLGrid glGrid(20,1.0, true);
glGraph.AddChild(&glGrid);
shadowLight.AddShadowReceiver(&glGrid);
// Define axis object, and set its pose
SceneGraph::GLAxis glAxis;
glAxis.SetPose(-1,-2,-0.1, 0, 0, M_PI/4);
glAxis.SetScale(0.25);
glGraph.AddChild(&glAxis);
// Define 3D spiral using a GLCachedPrimitives object
SceneGraph::GLCachedPrimitives glSpiral(GL_LINE_STRIP, SceneGraph::GLColor(1.0f,0.7f,0.2f));
for(double t=0; t < 10*M_PI; t+= M_PI/50) {
glSpiral.AddVertex(Eigen::Vector3d(cos(t)+2, sin(t)+2, -0.2*t) );
}
glGraph.AddChild(&glSpiral);
SceneGraph::GLCube glCube;
glCube.SetPose(1.5,1.5,-sqrt(3), M_PI/4, M_PI/4, M_PI/4);
glGraph.AddChild(&glCube);
shadowLight.AddShadowCaster(&glCube);
#ifdef HAVE_ASSIMP
// Define a mesh object and try to load model
SceneGraph::GLMesh glMesh;
try {
glMesh.Init("./model.blend");
glMesh.SetPosition(0,0,-1);
glMesh.SetScale(4.0f);
glGraph.AddChild(&glMesh);
shadowLight.AddShadowCasterAndReceiver(&glMesh);
}catch(exception e) {
cerr << "Cannot load mesh. Check file exists" << endl;
}
#endif // HAVE_ASSIMP
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState stacks3d(
pangolin::ProjectionMatrix(640,480,420,420,320,240,0.1,1000),
pangolin::ModelViewLookAt(0,-2,-4, 0,1,0, pangolin::AxisNegZ)
);
// Pangolin abstracts the OpenGL viewport as a View.
// Here we get a reference to the default 'base' view.
pangolin::View& container = pangolin::DisplayBase();
// We define a new view which will reside within the container.
pangolin::View view3d;
// We set the views location on screen and add a handler which will
// let user input update the model_view matrix (stacks3d) and feed through
// to our scenegraph
view3d.SetBounds(0.0, 1.0, 0.0, 1.0, 640.0f/480.0f)
.SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d,pangolin::AxisNegZ))
.SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));
pangolin::View& viewLight = pangolin::CreateDisplay()
.SetBounds(0.0, 0.3, 0, 0.3)
.SetDrawFunction(SceneGraph::ActivateScissorClearDrawFunctor(glGraph,shadowLight.GetRenderState()));
// Add our view as children to the base container.
container.AddDisplay(view3d);
container.AddDisplay(viewLight);
// Default hooks for exiting (Esc) and fullscreen (tab).
for(int frame=0; !pangolin::ShouldQuit(); ++frame )
{
// Clear whole screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Animate position of light over time.
shadowLight.SetPosition(100*cos(frame/100.0), 100*sin(frame/100.0), -100 );
// Swap frames and Process Events
pangolin::FinishFrame();
// Pause for 1/60th of a second.
std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 60));
}
return 0;
}
int main( int argc, char* argv[] )
{
const unsigned int w = 512;
const unsigned int h = 512;
const roo::ImageIntrinsics K(500,500,w /2, h /2);
const int volres = 128;
// Initialise window
View& container = SetupPangoGLWithCuda(2*w, h);
SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
Var<float> near("ui.near",0, 0, 10);
Var<float> far("ui.far",100, 0, 100);
// Allocate Camera Images on device for processing
roo::Image<float, roo::TargetDevice, roo::Manage> img(w,h);
roo::Image<float, roo::TargetDevice, roo::Manage> depth(w,h);
roo::Image<float4, roo::TargetDevice, roo::Manage> norm(w,h);
roo::Image<float, roo::TargetDevice, roo::Manage> gtd(w,h);
roo::Image<float4, roo::TargetDevice, roo::Manage> gtn(w,h);
pangolin::GlBufferCudaPtr vbo(pangolin::GlArrayBuffer, w*h,GL_FLOAT,4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW);
roo::BoundedVolume<roo::SDF_t, roo::TargetDevice, roo::Manage> vol(volres,volres,volres,make_float3(-1,-1,-1), make_float3(1,1,1));
ActivateDrawImage<float> adg(img, GL_LUMINANCE32F_ARB, true, true);
ActivateDrawImage<float4> adn(norm, GL_RGBA32F, true, true);
ActivateDrawImage<float4> adin(gtn, GL_RGBA32F, true, true);
SceneGraph::GLSceneGraph graph;
SceneGraph::GLAxis glaxis;
SceneGraph::GLAxisAlignedBox glbox;
SceneGraph::GLVbo glvbo(&vbo);
graph.AddChild(&glaxis);
graph.AddChild(&glbox);
graph.AddChild(&glvbo);
pangolin::OpenGlRenderState stacks_view(
ProjectionMatrixRDF_TopLeft(w,h, K.fu,K.fv,K.u0,K.v0, 1E-2,1E3),
ModelViewLookAtRDF(0,0,-4,0,0,1,0,-1,0)
);
pangolin::OpenGlRenderState stacks_capture(
ProjectionMatrixRDF_TopLeft(w,h, K.fu,K.fv,K.u0,K.v0, 1E-2,1E3),
ModelViewLookAtRDF(0,0,-4,0,0,1,0,-1,0)
);
Handler3DDepth<float,roo::TargetDevice> handler(depth,stacks_view, AxisNone);
SceneGraph::HandlerSceneGraph handlerView(graph, stacks_view, AxisNone);
SceneGraph::HandlerSceneGraph handlerCapture(graph, stacks_capture, AxisNone);
SetupContainer(container, 5, (float)w/h);
container[0].SetDrawFunction(std::ref(adg)).SetHandler(&handler);
container[1].SetDrawFunction(std::ref(adn)).SetHandler(&handler);
container[2].SetDrawFunction(std::ref(adin)).SetHandler(&handler);
container[3].SetDrawFunction(SceneGraph::ActivateDrawFunctor(graph, stacks_view)).SetHandler( &handlerView );
container[4].SetDrawFunction(SceneGraph::ActivateDrawFunctor(graph, stacks_capture)).SetHandler( &handlerCapture );
const float voxsize = length(vol.VoxelSizeUnits());
roo::SdfSphere(vol, make_float3(0,0,0), 0.9 );
Var<bool> subpix("ui.subpix", true, true);
Var<bool> sdfreset("ui.reset", false, false);
Var<int> shape("ui.shape", 0, 0,1);
Var<bool> sdfsphere("ui.sphere", false, false);
Var<bool> fuse("ui.fuse", false, true);
Var<bool> fuseonce("ui.fuse once", false, false);
Var<float> trunc_dist("ui.trunc dist", 2*voxsize, 0, 2*voxsize);
Var<float> max_w("ui.max w", 10, 1E-4, 10);
Var<float> mincostheta("ui.min cos theta", 0.5, 0, 1);
Var<bool> test("ui.test", false, true);
Var<float> scale("ui.scale", 1, 0,100);
for(unsigned long frame=0; !pangolin::ShouldQuit(); ++frame)
{
if(test) {
stacks_capture.SetModelViewMatrix(stacks_view.GetModelViewMatrix());
}
Sophus::SE3d T_vw(stacks_view.GetModelViewMatrix());
Sophus::SE3d T_cw(stacks_capture.GetModelViewMatrix());
if(Pushed(sdfreset)) {
roo::SdfReset(vol, trunc_dist);
}
if(Pushed(sdfsphere)) {
roo::SdfSphere(vol, make_float3(0,0,0), 0.9 );
}
// Raycast current view
{
roo::RaycastSdf(depth, norm, img, vol, T_vw.inverse().matrix3x4(), K, near, far, trunc_dist, subpix );
}
// Generate depthmap by raycasting against groundtruth object
{
if(shape == 0) {
roo::RaycastBox(gtd, T_cw.inverse().matrix3x4(), K, roo::BoundingBox(make_float3(-0.9,-0.9,-0.9), make_float3(0.9,0.9,0.9)) );
}else if(shape ==1) {
roo::Fill<float>(gtd, std::numeric_limits<float>::quiet_NaN() );
roo::RaycastSphere(gtd, roo::Image<float>(), T_cw.inverse().matrix3x4(), K, make_float3(0,0,0), 0.9);
}
CudaScopedMappedPtr dvbo(vbo);
roo::Image<float4> vboimg((float4*)*dvbo,w,h);
roo::DepthToVbo<float>(vboimg, gtd, K, 1.0f);
roo::NormalsFromVbo(gtn,vboimg);
glvbo.SetPose(T_cw.inverse().matrix());
}
if(Pushed(fuseonce) || fuse) {
// integrate gtd into TSDF
roo::SdfFuse(vol, gtd, gtn, T_cw.matrix3x4(), K, trunc_dist, max_w, mincostheta );
}
if(test) {
// override img with difference between depth and gtd
roo::ElementwiseAdd<float,float,float,float>(img, depth, gtd, 1.0f, -1.0f, 0.0f);
// roo::ElementwiseSquare<float,float,float>(img,img);
roo::ElementwiseScaleBias<float,float,float>(img,img,scale);
}
/////////////////////////////////////////////////////////////
// Perform drawing
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f(1,1,1);
pangolin::FinishFrame();
}
}
int main( int /*argc*/, char** /*argv*/ )
{
// Create OpenGL window in single line thanks to GLUT
pangolin::CreateWindowAndBind("Main",640,480);
SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
glewInit();
// Scenegraph to hold GLObjects and relative transformations
SceneGraph::GLSceneGraph glGraph;
SceneGraph::GLLight light(10,10,-100);
glGraph.AddChild(&light);
SceneGraph::GLGrid grid(10,1,true);
glGraph.AddChild(&grid);
SceneGraph::GLCube cube;
cube.SetPose(0,0,20,M_PI_4,M_PI_4,0.1);
glGraph.AddChild(&cube);
const SceneGraph::AxisAlignedBoundingBox bbox = glGraph.ObjectAndChildrenBounds();
const Eigen::Vector3d center = bbox.Center();
const double size = bbox.Size().norm();
const double far = 10*size;
const double near = far / 1E3;
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState stacks3d(
pangolin::ProjectionMatrix(640,480,420,420,320,240,near,far),
pangolin::ModelViewLookAt(center(0), center(1) + size, center(2) + size/4, center(0), center(1), center(2), pangolin::AxisZ)
);
// We define a new view which will reside within the container.
pangolin::View view3d;
// We set the views location on screen and add a handler which will
// let user input update the model_view matrix (stacks3d) and feed through
// to our scenegraph
view3d.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
.SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d))
.SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));
// Add our views as children to the base container.
pangolin::DisplayBase().AddDisplay(view3d);
// Physics stuff
// Build the broadphase (approximate collision detection)
btDbvtBroadphase broadphase;
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btSequentialImpulseConstraintSolver solver;
btDiscreteDynamicsWorld dynamicsWorld(&dispatcher,&broadphase,&solver,&collisionConfiguration);
dynamicsWorld.setGravity(btVector3(0,0,-10));
btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,0,1),0);
btCollisionShape* fallShape = new btBoxShape(toBulletVec3( cube.ObjectBounds().HalfSizeFromOrigin() ) );
btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,0)));
btRigidBody::btRigidBodyConstructionInfo
groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
groundRigidBody->setRestitution(0.1);
dynamicsWorld.addRigidBody(groundRigidBody);
SceneGraphMotionState* fallMotionState = new SceneGraphMotionState(cube);
btScalar mass = 1;
btVector3 fallInertia(0,0,0);
fallShape->calculateLocalInertia(mass,fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
btRigidBody* fallRigidBody = new btRigidBody(fallRigidBodyCI);
fallRigidBody->setRestitution(5);
dynamicsWorld.addRigidBody(fallRigidBody);
// Default hooks for exiting (Esc) and fullscreen (tab).
while( !pangolin::ShouldQuit() )
{
// Clear whole screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Swap frames and Process Events
pangolin::FinishFrame();
// Pause for 1/60th of a second.
usleep(1E6 / 60);
dynamicsWorld.stepSimulation(1/60.f,10);
}
dynamicsWorld.removeRigidBody(fallRigidBody);
delete fallRigidBody->getMotionState();
delete fallRigidBody;
dynamicsWorld.removeRigidBody(groundRigidBody);
delete groundRigidBody->getMotionState();
delete groundRigidBody;
delete fallShape;
delete groundShape;
return 0;
}
