Eigen::Matrix4f Helper::convertToEigenMatrix(const Matrix4 &mat) { Eigen::Matrix4f M; M << mat.M11, mat.M12, mat.M13, mat.M14, mat.M21, mat.M22, mat.M23, mat.M24, mat.M31, mat.M32, mat.M33, mat.M34, mat.M41, mat.M42, mat.M43, mat.M44; return M.transpose(); }
HRESULT DX11BuildLinearSystem::applyBL( ID3D11DeviceContext* context, ID3D11ShaderResourceView* inputSRV, ID3D11ShaderResourceView* correspondenceSRV, ID3D11ShaderResourceView* correspondenceNormalsSRV, D3DXVECTOR3& mean, float meanStDev, Eigen::Matrix4f& deltaTransform, unsigned int imageWidth, unsigned int imageHeight, unsigned int level, Matrix6x7f& res, LinearSystemConfidence& conf ) { HRESULT hr = S_OK; unsigned int dimX = (unsigned int)ceil(((float)imageWidth*imageHeight)/(GlobalCameraTrackingState::getInstance().s_localWindowSize[level]*m_blockSizeBL)); Eigen::Matrix4f deltaTransformT = deltaTransform.transpose(); // Initialize Constant Buffer D3D11_MAPPED_SUBRESOURCE mappedResource; V_RETURN(context->Map(m_constantBufferBL, 0, D3D11_MAP_WRITE_DISCARD, 0, &mappedResource)) CBufferBL *cbuffer = (CBufferBL*)mappedResource.pData; cbuffer->imageWidth = (int)imageWidth; cbuffer->imageHeigth = (int)imageHeight; memcpy(cbuffer->deltaTransform, deltaTransformT.data(), 16*sizeof(float)); cbuffer->imageHeigth = (int)imageHeight; memcpy(cbuffer->mean, (float*)mean, 3*sizeof(float)); cbuffer->meanStDevInv = 1.0f/meanStDev; context->Unmap(m_constantBufferBL, 0); // Setup Pipeline context->CSSetShaderResources(0, 1, &inputSRV); context->CSSetShaderResources(1, 1, &correspondenceSRV); context->CSSetShaderResources(2, 1, &correspondenceNormalsSRV); context->CSSetUnorderedAccessViews(0, 1, &m_pOutputFloatUAV, 0); context->CSSetConstantBuffers(0, 1, &m_constantBufferBL); context->CSSetShader(m_pComputeShaderBL[level], 0, 0); // Start Compute Shader context->Dispatch(dimX, 1, 1); assert(dimX <= D3D11_CS_DISPATCH_MAX_THREAD_GROUPS_PER_DIMENSION); // De-Initialize Pipeline ID3D11ShaderResourceView* nullSAV[1] = { NULL }; ID3D11UnorderedAccessView* nullUAV[1] = { NULL }; ID3D11Buffer* nullCB[1] = { NULL }; context->CSSetShaderResources(0, 1, nullSAV); context->CSSetShaderResources(1, 1, nullSAV); context->CSSetShaderResources(2, 1, nullSAV); context->CSSetUnorderedAccessViews(0, 1, nullUAV, 0); context->CSSetConstantBuffers(0, 1, nullCB); context->CSSetShader(0, 0, 0); // Copy to CPU context->CopyResource(m_pOutputFloatCPU, m_pOutputFloat); V_RETURN(context->Map(m_pOutputFloatCPU, 0, D3D11_MAP_READ, 0, &mappedResource)); res = reductionSystemCPU((float*)mappedResource.pData, dimX, conf); context->Unmap(m_pOutputFloatCPU, 0); return hr; }
void GLWidget::paintGL (void) { makeCurrent(); // Clear the screen glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); Eigen::Vector4f viewport = camera->getViewport(); glViewport(viewport[0], viewport[1], viewport[2], viewport[3]); shader.bind(); if (height_map[currentMap] != nullptr) shader.setUniform("in_HeightMap", height_map[currentMap]->bind()); // sets all uniform variables for the phong shader Eigen::Matrix4f modelViewMatrix = (camera->getViewMatrix() * terrainMesh.getModelMatrix()).matrix(); shader.setUniform("modelViewMatrix", modelViewMatrix); Eigen::Matrix3f normalMatrix = modelViewMatrix.transpose().inverse().matrix().block<3, 3>(0, 0); shader.setUniform("normalMatrix", normalMatrix); shader.setUniform("projectionMatrix", camera->getProjectionMatrix()); shader.setUniform("lightViewMatrix", light_trackball->getViewMatrix()); shader.setUniform("tessLevelInner", tessInnerLevel); shader.setUniform("tessLevelOuter", tessOuterLevel); shader.setUniform("depthLevel", depthLevel); shader.setUniform("MinTessLevel", minTessLevel); shader.setUniform("MaxTessLevel", maxTessLevel); shader.setUniform("MinDepth", minDepth); shader.setUniform("MaxDepth", maxDepth); shader.setUniform("wireframeEnabled", wireframeEnabled); shader.setUniform("showNormals", showNormals); terrainMesh.setAttributeLocation(shader); glEnable(GL_DEPTH_TEST); terrainMesh.render(); shader.unbind(); if (height_map[currentMap] != nullptr) height_map[currentMap]->unbind(); }
bool CameraPoseFinderICP::estimateCameraPose(const DepthFrameData& depth_frame,const ColorFrameData& color_frame) { if(depth_frame.frameId()==0) { return true; } //prepare datas cudaDownSampleNewVertices(); cudaDownSampleNewNormals(); cudaDownSampleModelVertices(); cudaDownSampleModelNormals(); Mat44 cur_transform=_pose; Mat44 last_transform_inv=_pose.getInverse(); Eigen::Matrix<float, 6, 1, 0, 6, 1> delta_dof; for(int l=_iter_nums.size()-1;l>=0;l--) { int it_nums=_iter_nums[l]; while(it_nums--) { // findCorresSet(l,cur_transform,last_transform_inv); if(false==minimizePointToPlaneErrFunc(l,delta_dof,cur_transform,last_transform_inv)) return false; Eigen::Matrix4f t = Eigen::Matrix4f::Identity(); if (false==vector6ToTransformMatrix(delta_dof, t) ) { cout<<"camera shaking detected"<<endl; return false; } //eigen matrix to mat44 Eigen::Matrix4f tt = t.transpose(); Mat44 mat_t(tt.data()); cur_transform = mat_t*cur_transform; } } _pose=cur_transform; return true; }
template <typename GraphT, typename PointT> void pcl::PairwiseGraphRegistration<GraphT, PointT>::computeRegistration () { if (!registration_method_) { PCL_ERROR ("[pcl::PairwiseGraphRegistration::computeRegistration] No registration method set!\n"); return; } typename std::vector<GraphHandlerVertex>::iterator last_vx_it = last_vertices_.begin (); if (last_aligned_vertex_ == boost::graph_traits<GraphT>::null_vertex ()) { last_aligned_vertex_ = *last_vx_it; ++last_vx_it; } pcl::PointCloud<PointT> fake_cloud; registration_method_->setInputTarget (boost::get_cloud<PointT> (last_aligned_vertex_, *(graph_handler_->getGraph ()))); for(; last_vx_it < last_vertices_.end (); ++last_vx_it) { registration_method_->setInputCloud (boost::get_cloud<PointT> (*last_vx_it, *(graph_handler_->getGraph ()))); const Eigen::Matrix4f last_aligned_vertex_pose = boost::get_pose (last_aligned_vertex_, *(graph_handler_->getGraph ())); if (!incremental_) { const Eigen::Matrix4f guess = last_aligned_vertex_pose.transpose () * boost::get_pose (*last_vx_it, *(graph_handler_->getGraph ())); registration_method_->align (fake_cloud, guess); } else registration_method_->align (fake_cloud); const Eigen::Matrix4f global_ref_final_tr = last_aligned_vertex_pose * registration_method_->getFinalTransformation (); boost::set_estimate<PointT> (*last_vx_it, global_ref_final_tr, *(graph_handler_->getGraph ())); last_aligned_vertex_ = *last_vx_it; registration_method_->setInputTarget (boost::get_cloud<PointT> (last_aligned_vertex_, *(graph_handler_->getGraph ()))); } }