C++ Matrix2f类代码示例

Matrix2f TeamPlayersLocator::rotateCovariance(const Matrix2f& matrix, const float angle)
{
  const float cosine = std::cos(angle);
  const float sine = std::sin(angle);
  const Matrix2f rotationMatrix = (Matrix2f() << cosine, -sine, sine, cosine).finished();
  return (rotationMatrix * matrix) * rotationMatrix.transpose();
}

	void Matrix2Test::testConstantFunctionDeterminant(){
		const Matrix2f matrix( 1.0f, 2.0f, 3.0f, 4.0f );
		
		const float result = matrix.Determinant();
		const float expected = -2;
		
		CPPUNIT_ASSERT_EQUAL( expected, result );
	}

	Matrix2f trans(){
		Matrix2f res = Matrix2f(0,column,row);

		res.setNum(nums[0][0],0,0);
		res.setNum(nums[0][1],1,0);
		res.setNum(nums[1][0],0,1);
		res.setNum(nums[1][1],1,1);

		return res;
	}

	Matrix2f mul(float b){
		Matrix2f res = Matrix2f(0,row,column);

		for(int i=0;i<row;i++){
			for(int j=0;j<column;j++){
					res.setNum(nums[i][j]*b, i,j);
			}
		}

		return res;
	}

void pixelMatrixToImage()
{
    Matrix2f pixels;
    pixels << 255, 63,
              127, 255;

    float *data = pixels.data();

    CImg<float> img(data, /* x dim */ 2, /* y dim */ 2, /* z dim */ 1, /* color dim */ 1);
    img/*.normalize()*/.resize(150.0,150.0).display();
}

int main(int, char**)
{
  cout.precision(3);
  Matrix2f m = Matrix2f::Random();
m = (m + m.adjoint()).eval();
JacobiRotation<float> J;
J.makeJacobi(m, 0, 1);
cout << "Here is the matrix m:" << endl << m << endl;
m.applyOnTheLeft(0, 1, J.adjoint());
m.applyOnTheRight(0, 1, J);
cout << "Here is the matrix J' * m * J:" << endl << m << endl;
  return 0;
}

	Matrix2f sub(Matrix2f b){
		if(row != b.getColumn() || column != b.getRow()){
			Util::myException("Matrix error\n\r");
		}

		Matrix2f tmp = Matrix2f();

		for(int i=0;i<row;i++){
			for(int j=0;j<column;j++){
				tmp.setNum(nums[i][j] - b.getNum(i,j) , i,j);
			}
		}
		return tmp;
	}

	Matrix2f inverse(){
		if(row!=2 || column != 2){
			Util::myException("Matrix error at inverse\n\r");
		}

		Matrix2f res = Matrix2f(0,2,2);

		float det = (nums[0][0]*nums[1][1] - nums[0][1]*nums[1][0]);

		res.setNum( nums[1][1]/det,0,0);
		res.setNum(-nums[0][1]/det,0,1);
		res.setNum(-nums[1][0]/det,1,0);
		res.setNum( nums[0][0]/det,1,1);

		return res;
	}

int main(int, char**)
{
    cout.precision(3);
    Matrix2f M = Matrix2f::Random();
    Matrix2f m;
    m = M;
    cout << "Here is the matrix m:" << endl << m << endl;
    cout << "Now we want to copy a column into a row." << endl;
    cout << "If we do m.col(1) = m.row(0), then m becomes:" << endl;
    m.col(1) = m.row(0);
    cout << m << endl << "which is wrong!" << endl;
    cout << "Now let us instead do m.col(1) = m.row(0).eval(). Then m becomes" << endl;
    m = M;
    m.col(1) = m.row(0).eval();
    cout << m << endl << "which is right." << endl;

    return 0;
}

bool Triangle::contains_point(Point3f point, float epsilon) {
	Vector3f v1 = p1 - p0;
	Vector3f v2 = p2 - p0;
	Vector3f expected = point - p0;
	
	Matrix2f A; 
	A << v1[0], v2[0], 
	     v1[1], v2[1];
	Vector2f expected2f; 
	expected2f << expected[0], 
	              expected[1];
	
	// so using barycentric coord, we remove the z values solve for [v1 v2]x = point - p0
	// afterwards using the calculated values, we see if the z values fit.
	// x must have values from 0-1
	Vector2f x = A.inverse() * expected2f;
	if(x[0] < 0.0 || 1.0 < x[0] || x[1] < 0.0 || 1.0 < x[1]) 
		return false;
	
	float expected_z = (x[0] * v1[2]) + (x[1] * v2[2]);
	return (expected[2] - epsilon) <= expected_z && expected_z <= (expected[2] + epsilon);
}

void CSphere16::_draw(const float3x3& mat, const bool reverse_normal)
{
	const Vector3f *pvert = (const Vector3f*)&vert[0];
	const Vector3f *pnorm = &m_normals[0];				
	const Vector3i *ptri = (Vector3i*)tris;
	
	//prepare vertex transform
	Matrix2f m; m.setIdentityMatrix();
	m.x[0][0] = mat.x[0];
	m.x[0][1] = mat.x[1];
	m.x[0][2] = mat.x[2];
	m.x[1][0] = mat.x[3];
	m.x[1][1] = mat.x[4];
	m.x[1][2] = mat.x[5];
	m.x[2][0] = mat.x[6];
	m.x[2][1] = mat.x[7];
	m.x[2][2] = mat.x[8];

	glMultMatrixf(&m.x[0][0]);
	//prepare normal array
	if (reverse_normal) pnorm = &m_normals_inv[0];		
	//render the mesh
	drawSurfaceUsingArray(pvert, pnorm, tris, NT, 3);
}

ColorWheel::Region ColorWheel::adjustPosition(const Vector2i &p, Region consideredRegions) {
    float x = p.x() - mPos.x(),
          y = p.y() - mPos.y(),
          w = mSize.x(),
          h = mSize.y();

    float cx = w*0.5f;
    float cy = h*0.5f;
    float r1 = (w < h ? w : h) * 0.5f - 5.0f;
    float r0 = r1 * .75f;

    x -= cx;
    y -= cy;

    float mr = std::sqrt(x*x + y*y);

    if ((consideredRegions & OuterCircle) &&
        ((mr >= r0 && mr <= r1) || (consideredRegions == OuterCircle))) {
        if (!(consideredRegions & OuterCircle))
            return None;
        mHue = std::atan(y / x);
        if (x < 0)
            mHue += NVG_PI;
        mHue /= 2*NVG_PI;

        if (mCallback)
            mCallback(color());

        return OuterCircle;
    }

    float r = r0 - 6;

    float ax = std::cos( 120.0f/180.0f*NVG_PI) * r;
    float ay = std::sin( 120.0f/180.0f*NVG_PI) * r;
    float bx = std::cos(-120.0f/180.0f*NVG_PI) * r;
    float by = std::sin(-120.0f/180.0f*NVG_PI) * r;

    typedef Eigen::Matrix<float,2,2>        Matrix2f;

    Eigen::Matrix<float, 2, 3> triangle;
    triangle << ax,bx,r,
                ay,by,0;
    triangle = Eigen::Rotation2D<float>(mHue * 2 * NVG_PI).matrix() * triangle;

    Matrix2f T;
    T << triangle(0,0) - triangle(0,2), triangle(0,1) - triangle(0,2),
         triangle(1,0) - triangle(1,2), triangle(1,1) - triangle(1,2);
    Vector2f pos { x - triangle(0,2), y - triangle(1,2) };

    Vector2f bary = T.colPivHouseholderQr().solve(pos);
    float l0 = bary[0], l1 = bary[1], l2 = 1 - l0 - l1;
    bool triangleTest = l0 >= 0 && l0 <= 1.f && l1 >= 0.f && l1 <= 1.f &&
                        l2 >= 0.f && l2 <= 1.f;

    if ((consideredRegions & InnerTriangle) &&
        (triangleTest || consideredRegions == InnerTriangle)) {
        if (!(consideredRegions & InnerTriangle))
            return None;
        l0 = std::min(std::max(0.f, l0), 1.f);
        l1 = std::min(std::max(0.f, l1), 1.f);
        l2 = std::min(std::max(0.f, l2), 1.f);
        float sum = l0 + l1 + l2;
        l0 /= sum;
        l1 /= sum;
        mWhite = l0;
        mBlack = l1;
        if (mCallback)
            mCallback(color());
        return InnerTriangle;
    }

    return None;
}

Matrix2f M = Matrix2f::Random();
Matrix2f m;
m = M;
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Now we want to replace m by its own transpose." << endl;
cout << "If we do m = m.transpose(), then m becomes:" << endl;
m = m.transpose();
cout << m << endl << "which is wrong!" << endl;
cout << "Now let us instead do m = m.transpose().eval(). Then m becomes" << endl;
m = M;
m = m.transpose().eval();
cout << m << endl << "which is right." << endl;

//----------------------------------------------------------------------------
void AffineTransform (int warmups, Ellipse2f& ellipse0, Ellipse2f& ellipse1)
{
	float random = 0.0f;
	for (int i = 0; i < warmups; ++i)
	{
		random = Mathf::SymmetricRandom();
	}

	Matrix2f A;
	Vector2f B;
	for (int row = 0; row < 2; ++row)
	{
		for (int col = 0; col < 2; ++col)
		{
			A[row][col] = Mathf::SymmetricRandom();
		}
		B[row] = Mathf::SymmetricRandom();
	}
	if (A.Determinant() < 0.0f)
	{
		A[0][0] = -A[0][0];
		A[0][1] = -A[0][1];
	}

	Matrix2f invA = A.Inverse();
	Vector2f K = invA*(ellipse0.Center - B);
	Matrix2f D(
	    1.0f/(ellipse0.Extent[0]*ellipse0.Extent[0]),
	    1.0f/(ellipse0.Extent[1]*ellipse0.Extent[1]));
	Matrix2f N = A.TransposeTimes(D)*A;
	Matrix2f R;
	N.EigenDecomposition(R, D);

	ellipse0.Center = K;
	ellipse0.Axis[0] = R.GetColumn(0);
	ellipse0.Axis[1] = R.GetColumn(1);
	ellipse0.Extent[0] = Mathf::InvSqrt(D[0][0]);
	ellipse0.Extent[1] = Mathf::InvSqrt(D[1][1]);

	K = invA*(ellipse1.Center - B);
	D = Matrix2f(
	        1.0f/(ellipse1.Extent[0]*ellipse1.Extent[0]),
	        1.0f/(ellipse1.Extent[1]*ellipse1.Extent[1]));
	N = A.TransposeTimes(D)*A;
	N.EigenDecomposition(R, D);

	ellipse1.Center = K;
	ellipse1.Axis[0] = R.GetColumn(0);
	ellipse1.Axis[1] = R.GetColumn(1);
	ellipse1.Extent[0] = Mathf::InvSqrt(D[0][0]);
	ellipse1.Extent[1] = Mathf::InvSqrt(D[1][1]);
}

void CDifodo::buildCoordinatesPyramidFast()
{
	const float max_depth_dif = 0.1f;
	
	//Push coordinates back
	depth_old.swap(depth);
	xx_old.swap(xx);
	yy_old.swap(yy);

	//The number of levels of the pyramid does not match the number of levels used
	//in the odometry computation (because we might want to finish with lower resolutions)

	unsigned int pyr_levels = round(log(float(width/cols))/log(2.f)) + ctf_levels;

	//Generate levels
	for (unsigned int i = 0; i<pyr_levels; i++)
	{
		unsigned int s = pow(2.f,int(i));
		cols_i = width/s;
		rows_i = height/s;
		//const int rows_i2 = 2*rows_i;
		//const int cols_i2 = 2*cols_i;
		const int i_1 = i-1;

		if (i == 0)
			depth[i].swap(depth_wf);

		//                              Downsampling
		//-----------------------------------------------------------------------------
		else
		{
			for (unsigned int u = 0; u < cols_i; u++)
				for (unsigned int v = 0; v < rows_i; v++)
				{
					const int u2 = 2*u;
					const int v2 = 2*v;
					
					//Inner pixels
					if ((v>0)&&(v<rows_i-1)&&(u>0)&&(u<cols_i-1))
					{
						const Matrix4f d_block = depth[i_1].block<4,4>(v2-1,u2-1);
						float depths[4] = {d_block(5),d_block(6),d_block(9),d_block(10)};
						float dcenter;

						//Sort the array (try to find a good/representative value)
						for (signed char k = 2; k>=0; k--)
						if (depths[k+1] < depths[k])
							std::swap(depths[k+1],depths[k]);
						for (unsigned char k = 1; k<3; k++)
						if (depths[k] > depths[k+1])
							std::swap(depths[k+1],depths[k]);
						if (depths[2] < depths[1])
							dcenter = depths[1];
						else
							dcenter = depths[2];
						
						if (dcenter > 0.f)
						{	
							float sum = 0.f;
							float weight = 0.f;

							for (unsigned char k = 0; k<16; k++)
							{
								const float abs_dif = abs(d_block(k) - dcenter);
								if (abs_dif < max_depth_dif)
								{
									const float aux_w = f_mask(k)*(max_depth_dif - abs_dif);
									weight += aux_w;
									sum += aux_w*d_block(k);
								}
							}
							depth[i](v,u) = sum/weight;
						}
						else
							depth[i](v,u) = 0.f;

                    }

                    //Boundary
					else
					{
						const Matrix2f d_block = depth[i_1].block<2,2>(v2,u2);
						const float new_d = 0.25f*d_block.sumAll();
						if (new_d < 0.4f)
							depth[i](v,u) = 0.f;
						else
							depth[i](v,u) = new_d;
					}
				}
        }

        //Calculate coordinates "xy" of the points
		const float inv_f_i = 2.f*tan(0.5f*fovh)/float(cols_i);
        const float disp_u_i = 0.5f*(cols_i-1);
        const float disp_v_i = 0.5f*(rows_i-1);

        for (unsigned int u = 0; u < cols_i; u++) 
			for (unsigned int v = 0; v < rows_i; v++)
                if (depth[i](v,u) > 0.f)
				{
//.........这里部分代码省略.........