示例#1
0
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;
}
示例#3
0
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 ())));
  }
}