void SceneNode::RenderShadowVolumes( const Affine3f & parentTransform )
{
const Affine3f worldTransform = parentTransform * m_Transform;
glLoadMatrixf( worldTransform.data() );
foreach( MeshInstance & instance, m_MeshInstances )
instance.RenderShadowVolumes();
foreach( SceneNode & child, m_ChildNodes )
child.RenderShadowVolumes( worldTransform );
}
void SgMesh::transform(const Affine3f& T)
{
if(hasVertices()){
auto& v = *vertices();
for(size_t i=0; i < v.size(); ++i){
v[i] = T.linear() * v[i] + T.translation();
}
if(hasNormals()){
auto& n = *normals();
for(size_t i=0; i < n.size(); ++i){
n[i] = T.linear() * n[i];
}
}
}
setPrimitive(MESH); // clear the primitive information
}
void on_draw_frame() {
//glClearColor(0.30f, 0.74f, 0.20f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT);
// Set the blending function (normal w/ premultiplied alpha)
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
update_life_cycle();
// Create Projection Matrix
float aspectRatio = 768.0f / 1022;
Affine3f m;
m = Scaling(1.0f, aspectRatio, 1.0f);
Matrix4f projectionMatrix = Matrix4f::Identity();
projectionMatrix = m.matrix();
// Render Emitter
if (s_emitters.size() > 0) {
std::vector<EmitterObject*>::iterator it = s_emitters.begin();
for (; it != s_emitters.end(); ++it) {
(*it)->RenderWithProjection(projectionMatrix);
}
}
/*
glUseProgram(program);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, texture);
glUniform1i(u_texture_unit_location, 0);
glBindBuffer(GL_ARRAY_BUFFER, buffer);
glVertexAttribPointer(a_position_location, 2, GL_FLOAT, GL_FALSE,
4 * sizeof(GL_FLOAT), BUFFER_OFFSET(0));
glVertexAttribPointer(a_texture_coordinates_location, 2, GL_FLOAT, GL_FALSE,
4 * sizeof(GL_FLOAT), BUFFER_OFFSET(2 * sizeof(GL_FLOAT)));
glEnableVertexAttribArray(a_position_location);
glEnableVertexAttribArray(a_texture_coordinates_location);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glBindBuffer(GL_ARRAY_BUFFER, 0);
*/
}
void DX11DeferredRendererMaterial::SetMatrix(const Affine3f& world, const Matrix4f& view_projection){
auto& buffer = *shader_parameters_->Lock<ShaderParameters>();
// Update
buffer.world = world.matrix();
buffer.world_view_proj = (view_projection * world).matrix();
shader_parameters_->Unlock();
}
void kfusion::cuda::TsdfVolume::integrate(const Dists& dists, tsdf_buffer& buffer, const Affine3f& camera_pose, const Intr& intr)
{
Affine3f vol2cam = camera_pose.inv() * pose_;
device::Projector proj(intr.fx, intr.fy, intr.cx, intr.cy);
device::Vec3i dims = device_cast<device::Vec3i>(dims_);
device::Vec3f vsz = device_cast<device::Vec3f>(getVoxelSize());
device::Aff3f aff = device_cast<device::Aff3f>(vol2cam);
device::TsdfVolume volume(data_.ptr<ushort2>(), dims, vsz, trunc_dist_, max_weight_);
device::integrate(dists, volume, buffer, aff, proj);
}
void PhongShadowProgram::setTransformMatrices(const Affine3& viewMatrix, const Affine3& modelMatrix, const Matrix4& PV)
{
const Affine3f VM = (viewMatrix * modelMatrix).cast<float>();
const Matrix3f N = VM.linear();
const Matrix4f PVM = (PV * modelMatrix.matrix()).cast<float>();
if(useUniformBlockToPassTransformationMatrices){
transformBlockBuffer.write(modelViewMatrixIndex, VM);
transformBlockBuffer.write(normalMatrixIndex, N);
transformBlockBuffer.write(MVPIndex, PVM);
transformBlockBuffer.flush();
} else {
glUniformMatrix4fv(modelViewMatrixLocation, 1, GL_FALSE, VM.data());
glUniformMatrix3fv(normalMatrixLocation, 1, GL_FALSE, N.data());
glUniformMatrix4fv(MVPLocation, 1, GL_FALSE, PVM.data());
for(int i=0; i < numShadows_; ++i){
ShadowInfo& shadow = shadowInfos[i];
const Matrix4f BPVM = (shadow.BPV * modelMatrix.matrix()).cast<float>();
glUniformMatrix4fv(shadow.shadowMatrixLocation, 1, GL_FALSE, BPVM.data());
}
}
}
void
pcl::gpu::RayCaster::run(const TsdfVolume& volume, const Affine3f& camera_pose)
{
camera_pose_.linear() = camera_pose.linear();
camera_pose_.translation() = camera_pose.translation();
volume_size_ = volume.getSize();
device::Intr intr (fx_, fy_, cx_, cy_);
vertex_map_.create(rows * 3, cols);
normal_map_.create(rows * 3, cols);
typedef Matrix<float, 3, 3, RowMajor> Matrix3f;
Matrix3f R = camera_pose_.linear();
Vector3f t = camera_pose_.translation();
const Mat33& device_R = device_cast<const Mat33>(R);
const float3& device_t = device_cast<const float3>(t);
float tranc_dist = volume.getTsdfTruncDist();
device::raycast (intr, device_R, device_t, tranc_dist, device_cast<const float3>(volume_size_), volume.data(), vertex_map_, normal_map_);
}
void Viewer::keyPressEvent(QKeyEvent *pEvent_)
{
using namespace btl::kinect;
using namespace Eigen;
// Defines the Alt+R shortcut.
if (pEvent_->key() == Qt::Key_0)
{
_pData->_bCameraFollow = !_pData->_bCameraFollow;
camera()->setPosition(qglviewer::Vec(0, 0, 0));
camera()->setUpVector(qglviewer::Vec(0, -1, 0));
camera()->setViewDirection(qglviewer::Vec(0, 0, 1));
updateGL(); // Refres
#ifdef INACCURATE_METHOD_0
Affine3f prj_w_t_c; _pData->_pTracker->getCurrentProjectionMatrix(&prj_w_t_c);
Vector3f pos = -prj_w_t_c.linear().transpose() * prj_w_t_c.translation();
Vector3f upv = prj_w_t_c.linear()*Vector3f(0, -1, 0); //up vector
Vector3f vdr = prj_w_t_c.linear()*Vector3f(0, 0, 1); //viewing direction
camera()->setPosition(qglviewer::Vec(pos(0), pos(1), pos(2)));
camera()->setUpVector(qglviewer::Vec(upv(0), upv(1), upv(2)));
camera()->setViewDirection(qglviewer::Vec(vdr(0), vdr(1), vdr(2)));
updateGL(); // Refresh display
#endif
#ifdef INACCURATE_METHOD_1
glMatrixMode(GL_MODELVIEW);
Affine3f init; init.setIdentity(); init(1, 1) = -1.f; init(2, 2) = -1.f;// rotate the default opengl camera orientation to make it facing positive z
Affine3f prj_w_t_c; _pData->_pTracker->getCurrentProjectionMatrix(&prj_w_t_c);
prj_w_t_c = init * prj_w_t_c; //the order matters
glLoadMatrixf(prj_w_t_c.data());//times with manipulation matrix
GLdouble mvm[16];
glGetDoublev(GL_MODELVIEW_MATRIX, mvm);
camera()->setFromModelViewMatrix(mvm);
updateGL(); // Refresh display
#endif
}
else if (pEvent_->key() == Qt::Key_BracketLeft)
{
//_idx_view--;
//Affine3f prj_w_t_c = _pData->getTrackerPtr()->getView(&_idx_view);
//Affine3f init; init.setIdentity(); init(1, 1) = -1.f; init(2, 2) = -1.f;// rotate the default opengl camera orientation to make it facing positive z
//prj_w_t_c = init * prj_w_t_c; //the order matters
//Affine3d mTmp = prj_w_t_c.cast<double>();
//mTmp.linear().transposeInPlace();
//camera()->setFromModelViewMatrix(mTmp.data());
camera()->setPosition(qglviewer::Vec(0, 0, 0));
camera()->setUpVector(qglviewer::Vec(0, -1, 0));
camera()->setViewDirection(qglviewer::Vec(0, 0, 1));
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_BracketRight)
{
//_idx_view++;
//Affine3f prj_c_f_w = _pData->getTrackerPtr()->getView(&_idx_view);
//Affine3f init; init.setIdentity(); init(1, 1) = -1.f; init(2, 2) = -1.f;// rotate the default opengl camera orientation to make it facing positive z
//prj_c_f_w = init * prj_c_f_w; //the order matters
//Affine3d mTmp = prj_c_f_w.cast<double>();
//mTmp.linear().transposeInPlace();
//camera()->setFromModelViewMatrix(mTmp.data());
//camera()->setPosition(qglviewer::Vec(0, 0, 0));
//camera()->setUpVector(qglviewer::Vec(0, -1, 0));
//camera()->setViewDirection(qglviewer::Vec(0, 0, 1));
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_1)
{
_bShowText = !_bShowText;
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_2)
{
//Matrix4f mModelView;
//Affine3f prj;
//_pData->getTrackerPtr()->getCurrentFeatureProjectionMatrix(&prj);
//mModelView = btl::utility::setModelViewGLfromPrj(prj);
//Matrix4d mTmp = mModelView.cast<double>();
//mTmp.linear().transposeInPlace();
//camera()->setFromModelViewMatrix(mTmp.data());
//updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_4 && !(pEvent_->modifiers() & Qt::ShiftModifier)){
_pData->switchShowTexts();
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_6) {
//_pData->switchShowMatchedFeaturesForRelocalisation();
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_7) {
_pData->switchShowSurfaces();
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_8) {
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_9) {
_pData->switchPyramid();
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_L && !(pEvent_->modifiers() & Qt::ShiftModifier)) {
_pData->switchLighting();
updateGL(); // Refresh display
}
else if (pEvent_->key() == Qt::Key_F2) {
_pData->switchImgPlane();
updateGL();
}
else if (pEvent_->key() == Qt::Key_F3) {
_pData->switchReferenceFrame();
updateGL();
}
else if (pEvent_->key() == Qt::Key_F4){
_pData->switchCurrentFrame();
updateGL();
}
else if (pEvent_->key() == Qt::Key_F5){
_pData->switchCameraPath();
updateGL();
}
else if (pEvent_->key() == Qt::Key_F6){
_pData->switchShowCamera();
updateGL();
}
else if (pEvent_->key() == Qt::Key_R && !(pEvent_->modifiers() & Qt::ShiftModifier)){
_pData->loadFromYml();
_pData->reset();
updateGL();
}
else if (pEvent_->key() == Qt::Key_A && !(pEvent_->modifiers() & Qt::ShiftModifier)){
//lower a
_pData->_bSave = !_pData->_bSave;
updateGL();
}
else if (pEvent_->key() == Qt::Key_S && !(pEvent_->modifiers() & Qt::ShiftModifier)){
_pData->switchCapturing();
updateGL();
}
else if (pEvent_->key() == Qt::Key_T && !(pEvent_->modifiers() & Qt::ShiftModifier)){//t
_pData->switchTrackOnly();
updateGL();
}
else if (pEvent_->key() == Qt::Key_T && (pEvent_->modifiers() & Qt::ShiftModifier)){ //T
_pData->_bToneMapper = !_pData->_bToneMapper;
updateGL();
}
else if (pEvent_->key() == Qt::Key_P && !(pEvent_->modifiers() & Qt::ShiftModifier)){
_pData->switchCapturing();
updateGL();
}
else if (pEvent_->key() == Qt::Key_P && (pEvent_->modifiers() & Qt::ShiftModifier)){
_pData->switchContinuous();
updateGL();
}
else if (pEvent_->key() == Qt::Key_Escape && !(pEvent_->modifiers() & Qt::ShiftModifier)){
QCoreApplication::instance()->quit();
}
else if (pEvent_->key() == Qt::Key_F && (pEvent_->modifiers() & Qt::ShiftModifier)){
if (!isFullScreen()){
toggleFullScreen();
}
else{
setFullScreen(false);
resize(1280, 960);
}
}
else if (pEvent_->key() == Qt::Key_M ){
updateGL();
}
else if (pEvent_->key() == Qt::Key_Z && !(pEvent_->modifiers() & Qt::ShiftModifier)){ //'z'
_pData->_bExportRT = true;
updateGL();
}
QGLViewer::keyPressEvent(pEvent_);
}
Matrix4f Camera::getViewMatrix() const {
Affine3f view;
view.setIdentity();
view.translate(-zoom*Z_AXIS).rotate(rotation).translate(-center);
return view.matrix();
}
int main(int argc, char **argv) {
int indexptr = 0, c;
string outPrefix = "";
bool zip = false, kspace = false, procpar = false;
Filters filterType = Filters::None;
float f_a = 0, f_q = 0;
Nifti::DataType dtype = Nifti::DataType::COMPLEX64;
Affine3f scale; scale = Scaling(1.f);
while ((c = getopt_long(argc, argv, short_options, long_options, &indexptr)) != -1) {
switch (c) {
case 0: break; // It was an option that just sets a flag.
case 'o': outPrefix = string(optarg); break;
case 'z': zip = true; break;
case 'k': kspace = true; break;
case 'm': dtype = Nifti::DataType::FLOAT32; break;
case 's': scale = Scaling(static_cast<float>(atof(optarg))); break;
case 'f':
switch (*optarg) {
case 'h':
filterType = Filters::Hanning;
f_a = 0.1;
break;
case 't':
filterType = Filters::Tukey;
f_a = 0.75;
f_q = 0.25;
break;
default:
cerr << "Unknown filter type: " << string(optarg, 1) << endl;
return EXIT_FAILURE;
}
break;
case 'a':
if (filterType == Filters::None) {
cerr << "No filter type specified, so f_a is invalid" << endl;
return EXIT_FAILURE;
}
f_a = atof(optarg);
break;
case 'q':
if (filterType != Filters::Tukey) {
cerr << "Filter type is not Tukey, f_q is invalid" << endl;
return EXIT_FAILURE;
}
f_q = atof(optarg);
break;
case 'p': procpar = true; break;
case 'v': verbose = true; break;
case '?': // getopt will print an error message
cout << usage << endl;
return EXIT_FAILURE;
default:
cout << "Unhandled option " << string(1, c) << endl;
return EXIT_FAILURE;
}
}
if ((argc - optind) <= 0) {
cout << usage << endl;
cout << "No .fids specified" << endl;
return EXIT_FAILURE;
}
while (optind < argc) {
string inPath(argv[optind++]);
size_t fileSep = inPath.find_last_of("/") + 1;
size_t fileExt = inPath.find_last_of(".");
if ((fileExt == string::npos) || (inPath.substr(fileExt) != ".fid")) {
cerr << inPath << " is not a valid .fid directory" << endl;
}
string outPath = outPrefix + inPath.substr(fileSep, fileExt - fileSep) + ".nii";
if (zip)
outPath = outPath + ".gz";
Agilent::FID fid(inPath);
string apptype = fid.procpar().stringValue("apptype");
string seqfil = fid.procpar().stringValue("seqfil");
if (apptype != "im3D") {
cerr << "apptype " << apptype << " not supported, skipping." << endl;
continue;
}
if (verbose) {
cout << fid.print_info() << endl;
cout << "apptype = " << apptype << endl;
cout << "seqfil = " << seqfil << endl;
}
/*
* Assemble k-Space
*/
MultiArray<complex<float>, 4> vols;
if (seqfil.substr(0, 5) == "mge3d") {
vols = reconMGE(fid);
} else if (seqfil.substr(0, 7) == "mp3rage") {
vols = reconMP2RAGE(fid);
} else {
cerr << "Recon for " << seqfil << " not implemented, skipping." << endl;
continue;
}
/*
* Build and apply filter
*/
MultiArray<float, 3> filter;
switch (filterType) {
case Filters::None: break;
case Filters::Hanning:
if (verbose) cout << "Building Hanning filter" << endl;
filter = Hanning3D(vols.dims().head(3), f_a);
break;
case Filters::Tukey:
if (verbose) cout << "Building Tukey filter" << endl;
filter = Tukey3D(vols.dims().head(3), f_a, f_q);
break;
}
if (filterType != Filters::None) {
if (verbose) cout << "Applying filter" << endl;
for (int v = 0; v < vols.dims()[3]; v++) {
MultiArray<complex<float>, 3> vol = vols.slice<3>({0,0,0,v},{-1,-1,-1,0});
ApplyFilter3D(vol,filter);
}
}
/*
* FFT
*/
if (!kspace) {
for (int v = 0; v < vols.dims()[3]; v++) {
if (verbose) cout << "FFTing vol " << v << endl;
MultiArray<complex<float>, 3> vol = vols.slice<3>({0,0,0,v},{-1,-1,-1,0});
phase_correct_3(vol, fid);
fft_shift_3(vol);
fft_X(vol);
fft_Y(vol);
fft_Z(vol);
fft_shift_3(vol);
}
}
if (verbose) cout << "Writing file: " << outPath << endl;
list<Nifti::Extension> exts;
if (procpar) {
if (verbose) cout << "Embedding procpar" << endl;
ifstream pp_file(inPath + "/procpar", ios::binary);
pp_file.seekg(ios::end);
size_t fileSize = pp_file.tellg();
pp_file.seekg(ios::beg);
vector<char> data; data.reserve(fileSize);
data.assign(istreambuf_iterator<char>(pp_file), istreambuf_iterator<char>());
exts.emplace_back(NIFTI_ECODE_COMMENT, data);
}
Affine3f xform = scale * fid.procpar().calcTransform();
ArrayXf voxdims = (Affine3f(xform.rotation()).inverse() * xform).matrix().diagonal();
Nifti::Header outHdr(vols.dims(), voxdims, dtype);
outHdr.setTransform(xform);
Nifti::File output(outHdr, outPath, exts);
output.writeVolumes(vols.begin(), vols.end(), 0, vols.dims()[3]);
output.close();
}
return 0;
}
void TextRenderer::draw(Matrix4f perspectiveMatrix, Matrix4f viewingMatrix, float zNear, float zFar, bool mode2D) {
std::lock_guard<std::mutex> lock(mMutex);
if(!mode2D)
throw Exception("TextRender is only implemented for 2D at the moment");
if(mView == nullptr)
throw Exception("TextRenderer need access to the view");
for(auto it : mDataToRender) {
Text::pointer input = std::static_pointer_cast<Text>(it.second);
uint inputNr = it.first;
// Check if a texture has already been created for this text
if (mTexturesToRender.count(inputNr) > 0 && mTextUsed[inputNr] == input)
continue; // If it has already been created, skip it
if (mTexturesToRender.count(inputNr) > 0) {
// Delete old texture
glDeleteTextures(1, &mTexturesToRender[inputNr]);
mTexturesToRender.erase(inputNr);
glDeleteVertexArrays(1, &mVAO[inputNr]);
mVAO.erase(inputNr);
}
Text::access access = input->getAccess(ACCESS_READ);
std::string text = access->getData();
// Font setup
QColor color(mColor.getRedValue() * 255, mColor.getGreenValue() * 255, mColor.getBlueValue() * 255, 255);
QFont font = QApplication::font();
font.setPointSize(mFontSize);
if (mStyle == STYLE_BOLD) {
font.setBold(true);
} else if (mStyle == STYLE_ITALIC) {
font.setItalic(true);
}
QFontMetrics metrics(font);
const int width = metrics.boundingRect(QString(text.c_str())).width() + 10;
const int height = mFontSize + 5;
// create the QImage and draw txt into it
QImage textimg(width, height, QImage::Format_RGBA8888);
textimg.fill(QColor(0, 0, 0, 0));
{
// Draw the text to the image
QPainter painter(&textimg);
painter.setBrush(color);
painter.setPen(color);
painter.setFont(font);
painter.drawText(0, mFontSize, text.c_str());
}
// Make texture of QImage
GLuint textureID;
glGenTextures(1, &textureID);
glBindTexture(GL_TEXTURE_2D, textureID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA,
textimg.width(), textimg.height(),
0, GL_RGBA, GL_UNSIGNED_BYTE, textimg.bits());
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
// Draw texture on a quad
// Delete old VAO
if(mVAO.count(inputNr) > 0)
glDeleteVertexArrays(1, &mVAO[inputNr]);
GLuint VAO_ID;
glGenVertexArrays(1, &VAO_ID);
mVAO[inputNr] = VAO_ID;
glBindVertexArray(VAO_ID);
float vertices[] = {
// vertex: x, y, z; tex coordinates: x, y
0.0f, (float)height, 0.0f, 0.0f, 0.0f,
(float)width, (float)height, 0.0f, 1.0f, 0.0f,
(float)width, 0.0f, 0.0f, 1.0f, 1.0f,
0.0f, 0.0f, 0.0f, 0.0f, 1.0f,
};
// Delete old VBO
if(mVBO.count(inputNr) > 0)
glDeleteBuffers(1, &mVBO[inputNr]);
// Create VBO
uint VBO;
glGenBuffers(1, &VBO);
mVBO[inputNr] = VBO;
glBindBuffer(GL_ARRAY_BUFFER, VBO);
glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 5 * sizeof(float), (void *) 0);
glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 5 * sizeof(float), (void *) (3 * sizeof(float)));
glEnableVertexAttribArray(0);
glEnableVertexAttribArray(1);
// Delete old EBO
if(mEBO.count(inputNr) > 0)
glDeleteBuffers(1, &mEBO[inputNr]);
// Create EBO
uint EBO;
glGenBuffers(1, &EBO);
mEBO[inputNr] = EBO;
uint indices[] = { // note that we start from 0!
0, 1, 3, // first triangle
1, 2, 3 // second triangle
};
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(indices), indices, GL_STATIC_DRAW);
glBindVertexArray(0);
mTexturesToRender[inputNr] = textureID;
mTextUsed[inputNr] = input;
}
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
activateShader();
for(auto it : mDataToRender) {
const uint inputNr = it.first;
Affine3f transform = Affine3f::Identity();
// Get width and height of texture
glBindTexture(GL_TEXTURE_2D, mTexturesToRender[inputNr]);
int width, height;
glGetTexLevelParameteriv(GL_TEXTURE_2D, 0, GL_TEXTURE_WIDTH, &width);
glGetTexLevelParameteriv(GL_TEXTURE_2D, 0, GL_TEXTURE_HEIGHT, &height);
const float scale = 1.0f / mView->width();
const float scale2 = 1.0f / mView->height();
const int padding = 15;
Vector3f position;
switch(mPosition) {
case POSITION_CENTER:
position = Vector3f(-width*scale/2.0f, -height*scale2/2.0f, 0); // Center
break;
case POSITION_TOP_CENTER:
position = Vector3f(-width*scale/2.0f, 1 - (height + padding)*scale2, 0); // Top center
break;
case POSITION_BOTTOM_CENTER:
position = Vector3f(-width*scale/2.0f, -1 + padding*scale2, 0); // Bottom center
break;
case POSITION_TOP_LEFT:
position = Vector3f(-1 + padding*scale, 1 - (height + padding)*scale2, 0); // Top left corner
break;
case POSITION_TOP_RIGHT:
position = Vector3f(1 - (width + padding)*scale, 1 - (height + padding)*scale2, 0); // Top right corner
break;
case POSITION_BOTTOM_LEFT:
position = Vector3f(-1 + padding*scale, -1 + padding*scale2, 0); // Bottom left corner
break;
case POSITION_BOTTOM_RIGHT:
position = Vector3f(1 - (width + padding)*scale, -1 + padding*scale2, 0); // Bottom right corner
break;
}
transform.translate(position);
transform.scale(Vector3f(scale, scale2, 0));
uint transformLoc = glGetUniformLocation(getShaderProgram(), "transform");
glUniformMatrix4fv(transformLoc, 1, GL_FALSE, transform.data());
transformLoc = glGetUniformLocation(getShaderProgram(), "perspectiveTransform");
glUniformMatrix4fv(transformLoc, 1, GL_FALSE, perspectiveMatrix.data());
transformLoc = glGetUniformLocation(getShaderProgram(), "viewTransform");
glUniformMatrix4fv(transformLoc, 1, GL_FALSE, viewingMatrix.data());
// Actual drawing
glBindVertexArray(mVAO[inputNr]);
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_INT, 0);
glBindTexture(GL_TEXTURE_2D, 0);
glBindVertexArray(0);
}
glDisable(GL_BLEND);
deactivateShader();
}
bool kfusion::KinFu::operator()(const kfusion::cuda::Depth& depth, const kfusion::cuda::Image& /*image*/)
{
const KinFuParams& p = params_;
const int LEVELS = icp_->getUsedLevelsNum();
cuda::computeDists(depth, dists_, p.intr);
cuda::depthBilateralFilter(depth, curr_.depth_pyr[0], p.bilateral_kernel_size, p.bilateral_sigma_spatial, p.bilateral_sigma_depth);
if (p.icp_truncate_depth_dist > 0)
kfusion::cuda::depthTruncation(curr_.depth_pyr[0], p.icp_truncate_depth_dist);
for (int i = 1; i < LEVELS; ++i)
cuda::depthBuildPyramid(curr_.depth_pyr[i-1], curr_.depth_pyr[i], p.bilateral_sigma_depth);
for (int i = 0; i < LEVELS; ++i)
#if defined USE_DEPTH
cuda::computeNormalsAndMaskDepth(p.intr, curr_.depth_pyr[i], curr_.normals_pyr[i]);
#else
cuda::computePointNormals(p.intr(i), curr_.depth_pyr[i], curr_.points_pyr[i], curr_.normals_pyr[i]);
#endif
cuda::waitAllDefaultStream();
//can't perform more on first frame
if (frame_counter_ == 0)
{
volume_->integrate(dists_, cyclical_.getBuffer(), poses_.back(), p.intr);
#if defined USE_DEPTH
curr_.depth_pyr.swap(prev_.depth_pyr);
#else
curr_.points_pyr.swap(prev_.points_pyr);
#endif
curr_.normals_pyr.swap(prev_.normals_pyr);
return ++frame_counter_, false;
}
///////////////////////////////////////////////////////////////////////////////////////////
// ICP
Affine3f affine; // cuur -> prev
{
//ScopeTime time("icp");
#if defined USE_DEPTH
bool ok = icp_->estimateTransform(affine, p.intr, curr_.depth_pyr, curr_.normals_pyr, prev_.depth_pyr, prev_.normals_pyr);
#else
bool ok = icp_->estimateTransform(affine, p.intr, curr_.points_pyr, curr_.normals_pyr, prev_.points_pyr, prev_.normals_pyr);
#endif
if (!ok)
return reset(), false;
}
poses_.push_back(poses_.back() * affine); // curr -> global
// check if we need to shift
if(checkForShift_)
has_shifted_ = cyclical_.checkForShift(volume_, getCameraPose(), params_.distance_camera_target , perform_shift_, perform_last_scan_, record_mode_);
perform_shift_ = false;
///////////////////////////////////////////////////////////////////////////////////////////
// Volume integration
Affine3f local_pose = Affine3f().translate(poses_.back().translation() - volume_->getPose().translation());
local_pose.rotate(poses_.back().rotation());
// We do not integrate volume if camera does not move.
float rnorm = (float)cv::norm(affine.rvec());
float tnorm = (float)cv::norm(affine.translation());
bool integrate = (rnorm + tnorm)/2 >= p.tsdf_min_camera_movement;
if (integrate)
{
//ScopeTime time("tsdf");
volume_->integrate(dists_, cyclical_.getBuffer(), poses_.back(), p.intr);
}
///////////////////////////////////////////////////////////////////////////////////////////
// Ray casting
{
//ScopeTime time("ray-cast-all");
#if defined USE_DEPTH
volume_->raycast(poses_.back(), cyclical_.getBuffer(), p.intr, prev_.depth_pyr[0], prev_.normals_pyr[0]);
for (int i = 1; i < LEVELS; ++i)
resizeDepthNormals(prev_.depth_pyr[i-1], prev_.normals_pyr[i-1], prev_.depth_pyr[i], prev_.normals_pyr[i]);
#else
//volume_->raycast(local_pose, p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]);
volume_->raycast(poses_.back(), cyclical_.getBuffer(), p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]);
for (int i = 1; i < LEVELS; ++i)
resizePointsNormals(prev_.points_pyr[i-1], prev_.normals_pyr[i-1], prev_.points_pyr[i], prev_.normals_pyr[i]);
#endif
cuda::waitAllDefaultStream();
}
return ++frame_counter_, true;
}
int main(int argc, char** argv) {
MatrixXf m(3,3);
m << 1,2,3,4,5,6,7,8,9;
Vector3f v = m.col(0);
Vector3f v2 = m.col(1);
Vector3f v3 = m.col(2);
cout<<"Mat = "<<m<<endl;
cout<<"vector = " << v<<endl;
MatrixXf n = (m.rowwise() -= v);
cout<<"Mat2 = "<<n<<endl;
Matrix3f r;
r.col(0) = v;
r.col(1) = v2;
r.col(2) = v3;
cout<<"Mat3 = "<<r<<endl;
r *= 2;
cout<<"Mat3 = "<<r<<endl;
cout<<"Mat3 = "<<m<<endl;
float arr[] = {1,2,3};
vector<float> w(arr, arr + sizeof(arr) / sizeof(float));
for(int i = 0; i < w.size(); i+=1)
cout<<w[i]<<"\t";
cout<<endl<<"---------------"<<endl;
/** Inverting a singular matrix. **/
m << 1,0,0,1,0,0,1,0,0;
cout<<" Should segfault/ get garbage: "<<m.determinant()<<endl;
/** Affine matrix in Eigen. */
MatrixXf ml(4,4);
ml << 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;
Affine3f t;
Vector3f d(10,20,30);
t.linear() = ml.transpose().block(0,0,3,3);
t.translation() = ml.block(0,3,3,1);
cout<<"Affine: "<<endl<<t.affine()<<endl;
Vector3f tt(1,2,3);
t.translation() += tt;
cout<<"Affine after translation: "<<endl<<t.affine()<<endl;
/** Blocks. */
MatrixXf matr(3,2);
matr << 1,2,3,4,5,6;
MatrixXf combo(4,2);
combo <<matr, MatrixXf::Ones(1,matr.cols());
cout<<"Matr = "<<matr<<endl;
cout<<"Comb = "<<combo<<endl;
MatrixXf rrr = combo.block(0,0,3,2);
cout<<"rrr = "<<rrr<<endl;
/** Filling up a matrix*/
std::cout<<"---------------------------------------"<<std::endl;
MatrixXf matF(5,3);
Vector3f vF(1,.3,3);
Vector3f vF1(.123,.2,.3);
Vector3f vF2(33.4,23.3,12.07);
Vector3f vF3(0.54,8.96,14.3);
Vector3f vF4(8.9,0.34,32.2);
matF.row(0) = vF;
matF.row(1) = vF1;
matF.row(2) = vF2;
matF.row(3) = vF3;
matF.row(4) = vF4;
RowVector3f means = matF.colwise().sum()/5;
MatrixXf centered = (matF.rowwise() - means);
cout<<"Matrix Fill = \n"<<matF<<std::endl;
cout<<"Means = \n"<<means<<std::endl;
cout<<"Centered = \n"<<centered<<std::endl;
Eigen::JacobiSVD<MatrixXf> svd(centered / sqrt(5), ComputeFullV);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
MatrixXf V = svd.matrixV();
Vector3f f1 = V.col(0);
Vector3f f2 = V.col(1);
Vector3f f3 = V.col(2);
cout << "f1(0)" << f1(0) << endl;
VectorXf faa(V.rows());
faa.setZero();
//for (int i= 0; i < V.rows(); i++)
V.col(2) = faa;
cout << "V " << V<< endl;
cout << "<v1,v2>" << f1.dot(f2) << endl;
cout << "<v1,v3>" << f1.dot(f3) << endl;
cout << "<v3,v2>" << f3.dot(f2) << endl;
std::cout<<"---------------------------------------"<<std::endl;
Vector3f o3(1,2,3), o2(4,5,6);
cout << "outer? : "<< o3.cwiseProduct(o2) <<endl;
MatrixXd mxx(3,3);
mxx << 1,0,0,0,2,0,0,0,3;
cout << "mxx : "<< endl<<mxx <<endl;
vector<double> v31(3);
VectorXd::Map(&v31[0], 3) = mxx.row(0);
cout << "v: "<<endl;
for(int i=0; i < 3; i++)
cout<< v31[i]<< " " <<endl;
cout<<endl;
v31[1] = 100;
cout << "v: "<<endl;
for(int i=0; i < 3; i++)
cout << v31[i]<< " " <<endl;
cout<<endl;
cout << " mxx : "<<endl <<mxx<<endl;
Matrix3d tmat = Matrix3d::Identity();
tmat(0,0) = 0;
tmat(1,1) = 10;
tmat(1,2) = 20;
cout << "tmat: " << tmat<<endl;
cout << "------------\n";
for (unsigned i=0; i < tmat.rows(); i++) {
double sum = tmat.row(i).sum() + numeric_limits<double>::min();
tmat.row(i) /= sum;
}
cout << tmat << endl;
MatrixXd src(4,3);
src<< 0,0,0,1,1,1,2,2,2,3,3,3;
MatrixXd target(1,3);
target<< 1,1,1;
cout <<"cloud err: "<< (src.rowwise() -target.row(0)).rowwise().squaredNorm().transpose()<<endl;
return 0;
}
