C++ Matrix3f::block方法代码示例

int main(int, char**)
{
  cout.precision(3);
  Matrix3f m;
m.row(0) << 1, 2, 3;
m.block(1,0,2,2) << 4, 5, 7, 8;
m.col(2).tail(2) << 6, 9;		    
std::cout << m;

  return 0;
}

int main()
{
    Matrix3f m;
    m << 1, 2, 3,
         4, 5, 6,
         7, 8, 9;
    std::cout << m << std::endl;

    RowVectorXd vec1(3);
    vec1 << 1, 2, 3;
    std::cout << "vec1 = " << vec1 << std::endl;

    RowVectorXd vec2(4);
    vec2 << 1, 4, 9, 16;;
    std::cout << "vec2 = " << vec2 << std::endl;

    RowVectorXd joined(7);
    joined << vec1, vec2;
    std::cout << "joined = " << joined << std::endl;


    MatrixXf matA(2, 2);
    matA << 1, 2, 3, 4;
    MatrixXf matB(4, 4);
    matB << matA, matA/10, matA/10, matA;
    std::cout << matB << std::endl;

    {
    Matrix3f m;
    m.row(0) << 1, 2, 3;
    m.block(1,0,2,2) << 4, 5, 7, 8;
    m.col(2).tail(2) << 6, 9;                   
    std::cout << m << std::endl;
    }

    return 0;
}

//.........这里部分代码省略.........
    error = error || !c.getReal("maxError",filterParams.maxError);
    error = error || !c.getReal("maxDepthDiff",filterParams.maxDepthDiff);
    error = error || !c.getReal("minInlierFraction",filterParams.minInlierFraction);
    error = error || !c.getReal("WorldPlaneSize",filterParams.WorldPlaneSize);
    error = error || !c.getUInt("numRetries",filterParams.numRetries);
    filterParams.runPolygonization = false;
    
    if(error){
      printf("Error Loading Plane Filtering Parameters!\n");
      exit(2);
    }
  }
*/ 
  
  {
    ConfigReader::SubTree c(config,"initialConditions");
    bool error = false;
    curMapName = string(c.getStr("mapName"));
    error = error || curMapName.length()==0;
    error = error || !c.getVec2f("loc",initialLoc);
    error = error || !c.getReal("angle", initialAngle);
    error = error || !c.getReal("locUncertainty", locUncertainty);
    error = error || !c.getReal("angleUncertainty", angleUncertainty);
    
    if(error){
      printf("Error Loading Initial Conditions!\n");
      exit(2);
    }
  }
  
  {
    ConfigReader::SubTree c(config,"motionParams");
    
    bool error = false;
    error = error || !c.getReal("Alpha1", motionParams.Alpha1);
    error = error || !c.getReal("Alpha2", motionParams.Alpha2);
    error = error || !c.getReal("Alpha3", motionParams.Alpha3);
    error = error || !c.getReal("kernelSize", motionParams.kernelSize);
    
    
    if(error){
      printf("Error Loading Predict Parameters!\n");
      exit(2);
    }
  }
  
  {
    ConfigReader::SubTree c(config,"lidarParams");
    
    bool error = false;
    // Laser sensor properties  //BASURA
    error = error || !c.getReal("angleResolution", lidarParams.angleResolution);
    error = error || !c.getReal("minAngle", lidarParams.minAngle);
    error = error || !c.getReal("maxAngle", lidarParams.maxAngle);
    error = error || !c.getInt("numRays", lidarParams.numRays);
    error = error || !c.getReal("maxRange", lidarParams.maxRange);
    error = error || !c.getReal("minRange", lidarParams.minRange);
    
    // Pose of laser sensor on robot	//BASURA
    vector2f laserToBaseTrans;
    float xRot, yRot, zRot;
    error = error || !c.getVec2f<vector2f>("laserLoc", laserToBaseTrans);
    error = error || !c.getReal("xRot", xRot);
    error = error || !c.getReal("yRot", yRot);
    error = error || !c.getReal("zRot", zRot);
    Matrix3f laserToBaseRot;
    laserToBaseRot = AngleAxisf(xRot, Vector3f::UnitX()) * AngleAxisf(yRot, Vector3f::UnitY()) * AngleAxisf(zRot, Vector3f::UnitZ());
    lidarParams.laserToBaseTrans = Vector2f(V2COMP(laserToBaseTrans));
    lidarParams.laserToBaseRot = laserToBaseRot.block(0,0,2,2);
    
    // Parameters related to observation update
    error = error || !c.getReal("logObstacleProb", lidarParams.logObstacleProb);
    error = error || !c.getReal("logOutOfRangeProb", lidarParams.logOutOfRangeProb);
    error = error || !c.getReal("logShortHitProb", lidarParams.logShortHitProb);
    error = error || !c.getReal("correlationFactor", lidarParams.correlationFactor);
    error = error || !c.getReal("lidarStdDev", lidarParams.lidarStdDev);
    error = error || !c.getReal("attractorRange", lidarParams.attractorRange);
    error = error || !c.getReal("kernelSize", lidarParams.kernelSize);
    
    // Parameters related to observation refine
    error = error || !c.getInt("minPoints", lidarParams.minPoints);
    error = error || !c.getInt("numSteps", lidarParams.numSteps);
    error = error || !c.getReal("etaAngle", lidarParams.etaAngle);
    error = error || !c.getReal("etaLoc", lidarParams.etaLoc);
    error = error || !c.getReal("maxAngleGradient", lidarParams.maxAngleGradient);
    error = error || !c.getReal("maxLocGradient", lidarParams.maxLocGradient);
    error = error || !c.getReal("minCosAngleError", lidarParams.minCosAngleError);
    error = error || !c.getReal("correspondenceMargin", lidarParams.correspondenceMargin);
    error = error || !c.getReal("minRefineFraction", lidarParams.minRefineFraction);
    
    lidarParams.initialize();
    
    if(error){
      printf("Error Loading Lidar Parameters!\n");
      exit(2);
    }
  }


}

/**************************************************************************************************
 *  Procecure                                                                                     *
 *                                                                                                *
 *  Description: moveHandRelativeToObject                                                         *
 *  Class      : ApproachMovementSpace                                                            *
 **************************************************************************************************/
Matrix4f ApproachMovementSpace::moveHandRelativeToObject(float theta, float phi, float rho, bool update)
{
	RobotPtr     hand = eef  -> getRobot();

	// Don't let the hand collide with the object
	if (rho < 10) rho = 10;

	// Calculate new XYZ coordinates for the hand
	Vector3f new_pos     = object -> getGlobalPose().topLeftCorner (3,3) * calculateCarthesianCoordinates(theta, phi, rho);
	         new_pos    += object -> getGlobalPose().topRightCorner(3,1);

	Matrix4f new_pose    = hand -> getGlobalPose();
	Matrix3f orientation;

	// Calculate First Vector of the orientation Matrix
		// It is colinear with the distance vector between object and hand
	orientation.block(0,0,3,1) = object -> getGlobalPose().topRightCorner(3,1) - new_pos;

	// Calculate the Remainder two orientation vectors
	Matrix3f inertia  = object -> getGlobalPose().topLeftCorner(3,3);
		     inertia *= object -> getInertiaMatrix();

	// Select two vectors from the objects inertia matrix
	     uint vectors_left  = 2;
	for (uint column        = 0; column < 3; column += 1)
	{
		Vector3f v1 = orientation.block(0,0,3,1);

		if( v1.dot( inertia.col(column) ) != 1)
		{
			orientation.block(0,vectors_left,3,1) = inertia.col(column);

			vectors_left -= 1;

			if (vectors_left == 0) break;
		}
	}

	// Check if two new vectors were included
	if ( vectors_left != 0)
	{
		cout << endl << "[Error] Function: moveHandRelativeToObject   |   The two vectors from the object's inertia matrix were not selected.";

		exit(-1);
	}

	// Make the orientation matrix orthonormalized
	orientation  = gramSchmidtNormalization3f(orientation);
	orientation *= offset.topLeftCorner(3,3);

	// Check if the new orientation is mirrored or not
		// If so, correct it
	if ( ( orientation.determinant() - (-1) <  0.01) &&
		 ( orientation.determinant() - (-1) > -0.01) )
	{
		orientation.col(1) *= -1;
	}

	// Update new hand pose
	new_pose.topLeftCorner (3,3) = orientation;
	new_pose.topRightCorner(3,1) = new_pos - orientation * offset.topRightCorner(3,1);

	// Set updated pose
	if (update) hand -> setGlobalPose(new_pose);

	return new_pose;
}