C++ TransformationParameters::cols方法代码示例

typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::RigidTransformation::correctParameters(const TransformationParameters& parameters) const
{
	TransformationParameters ortho = parameters;
	if(ortho.cols() == 4)
	{
		const Eigen::Matrix<T, 3, 1> col0 = parameters.block(0, 0, 3, 1).normalized();
		const Eigen::Matrix<T, 3, 1> col1 = parameters.block(0, 1, 3, 1).normalized();
		const Eigen::Matrix<T, 3, 1> col2 = parameters.block(0, 2, 3, 1).normalized();


		ortho.block(0, 0, 3, 1) = col1.cross(col2);
		ortho.block(0, 1, 3, 1) = col2.cross(col0);
		ortho.block(0, 2, 3, 1) = col2;
	}
	else if(ortho.cols() == 3)
	{
		// R = [ a b]
		//     [-b a]
		
		// mean of a and b
		T a = (parameters(0,0) + parameters(1,1))/2; 	
		T b = (-parameters(1,0) + parameters(0,1))/2;
		T sum = sqrt(pow(a,2) + pow(b,2));

		a = a/sum;
		b = b/sum;

		ortho(0,0) =  a; ortho(0,1) = b;
		ortho(1,0) = -b; ortho(1,1) = a;
	}


	return ortho;
}

bool TransformationsImpl<T>::RigidTransformation::checkParameters(const TransformationParameters& parameters) const
{
	//FIXME: FP - should we put that as function argument?
	const T epsilon = 0.001;

	const TransformationParameters R(parameters.topLeftCorner(parameters.rows()-1, parameters.cols()-1));
	
	if(anyabs(1 - R.determinant()) > epsilon)
		return false;
	else
		return true;
}

typename PointMatcher<T>::DataPoints TransformationsImpl<T>::RigidTransformation::compute(
	const DataPoints& input,
	const TransformationParameters& parameters) const
{
	typedef typename PointMatcher<T>::Matrix Matrix;
	
	assert(input.features.rows() == parameters.rows());
	assert(parameters.rows() == parameters.cols());

	const unsigned int nbRows = parameters.rows()-1;
	const unsigned int nbCols = parameters.cols()-1;

	const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));

	if(this->checkParameters(parameters) == false)	
		throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal.");	
	
	DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.timeLabels, input.features.cols());
	
	// Apply the transformation to features
	transformedCloud.features = parameters * input.features;
	
	// Apply the transformation to descriptors
	int row(0);
	const int descCols(input.descriptors.cols());
	for (size_t i = 0; i < input.descriptorLabels.size(); ++i)
	{
		const int span(input.descriptorLabels[i].span);
		const std::string& name(input.descriptorLabels[i].text);
		const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols));
		BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols));
		if (name == "normals" || name == "observationDirections")
			outputDesc = R * inputDesc;
		else
			outputDesc = inputDesc;
		row += span;
	}
	
	return transformedCloud;
}

typename PointMatcher<T>::DataPoints TransformationsImpl<T>::PureTranslation::compute(const DataPoints& input,
		const TransformationParameters& parameters) const {
	assert(input.features.rows() == parameters.rows());
	assert(parameters.rows() == parameters.cols());

	if(this->checkParameters(parameters) == false)
		throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part  not identity.");

	DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.features.cols());

	// Apply the transformation to features
	transformedCloud.features = parameters * input.features;

	return transformedCloud;
}

bool TransformationsImpl<T>::PureTranslation::checkParameters(
		const TransformationParameters& parameters) const {
	const int rows = parameters.rows();
	const int cols = parameters.cols();

	// make a copy of parameters to perform the check
	TransformationParameters parameters_(parameters);

	// set the translation components of the transformation matrix to 0
	parameters_.block(0,cols-1,rows-1,1).setZero();

	// If we have the identity matrix, than this is indeed a pure translation
	if (parameters_.isApprox(TransformationParameters::Identity(rows,cols)))
		return true;
	else
		return false;
}

typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::PureTranslation::correctParameters(
		const TransformationParameters& parameters) const {
	const int rows = parameters.rows();
	const int cols = parameters.cols();

	// make a copy of the parameters to perform corrections on
	TransformationParameters correctedParameters(parameters);

	// set the top left block to the identity matrix
	correctedParameters.block(0,0,rows-1,cols-1).setIdentity();

	// fix the bottom row
	correctedParameters.block(rows-1,0,1,cols-1).setZero();
	correctedParameters(rows-1,cols-1) = 1;

	return correctedParameters;
}

typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::TransformationWithScale::correctParameters(
		const TransformationParameters& parameters) const {
	std::cout << "Correcting Paramsa. .. .. . . " << std::endl;

	const int rows = parameters.rows();
	const int cols = parameters.cols();

	TransformationParameters correctedParameters(parameters);
	return correctedParameters;

	// make a copy of the parameters to perform corrections on
	// // set the top left block to the identity matrix
	// correctedParameters.block(0,0,rows-1,cols-1).setIdentity();

	// // fix the bottom row
	// correctedParameters.block(rows-1,0,1,cols-1).setZero();
	// correctedParameters(rows-1,cols-1) = 1;

	// return correctedParameters;
}