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;
}
void SpecificWorker::LoadParameters()
{
WatchFiles watch_files;
ConfigReader config("/home/robocomp/robocomp/components/robocomp-robolab/experimental/CGR/etc/"); //path a los ficheros de configuracion desde el path del binario.
config.init(watch_files);
config.addFile("localization_parameters.cfg");
config.addFile("kinect_parameters.cfg");
if(!config.readFiles())
{
printf("Failed to read config\n");
exit(1);
}
/**
{
ConfigReader::SubTree c(config,"KinectParameters");
unsigned int maxDepthVal;
bool error = false;
error = error || !c.getReal("f",kinectDepthCam.f);
error = error || !c.getReal("fovH",kinectDepthCam.fovH);
error = error || !c.getReal("fovV",kinectDepthCam.fovV);
error = error || !c.getInt("width",kinectDepthCam.width);
error = error || !c.getInt("height",kinectDepthCam.height);
error = error || !c.getUInt("maxDepthVal",maxDepthVal);
kinectDepthCam.maxDepthVal = maxDepthVal;
vector3f kinectLoc;
float xRot, yRot, zRot;
error = error || !c.getVec3f("loc",kinectLoc);
error = error || !c.getReal("xRot",xRot);
error = error || !c.getReal("yRot",yRot);
error = error || !c.getReal("zRot",zRot);
kinectToRobotTransform.xyzRotationAndTransformation(xRot,yRot,zRot,kinectLoc);
if(error){
printf("Error Loading Kinect Parameters!\n");
exit(2);
}
}
{
ConfigReader::SubTree c(config,"PlaneFilteringParameters");
bool error = false;
error = error || !c.getUInt("maxPoints",filterParams.maxPoints);
error = error || !c.getUInt("numSamples",filterParams.numSamples);
error = error || !c.getUInt("numLocalSamples",filterParams.numLocalSamples);
error = error || !c.getUInt("planeSize",filterParams.planeSize);
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;
}