void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
IK_QSegment *qseg = (IK_QSegment *)seg;
Vector3d mstart(start[0], start[1], start[2]);
// convert from blender column major
Matrix3d mbasis = CreateMatrix(basis[0][0], basis[1][0], basis[2][0],
basis[0][1], basis[1][1], basis[2][1],
basis[0][2], basis[1][2], basis[2][2]);
Matrix3d mrest = CreateMatrix(rest[0][0], rest[1][0], rest[2][0],
rest[0][1], rest[1][1], rest[2][1],
rest[0][2], rest[1][2], rest[2][2]);
double mlength(length);
if (qseg->Composite()) {
Vector3d cstart(0, 0, 0);
Matrix3d cbasis;
cbasis.setIdentity();
qseg->SetTransform(mstart, mrest, mbasis, 0.0);
qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
}
else
qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
const Matrix3d VectorMath::TMatrix(const Vector3d &v)
{
double vnormsq = v.dot(v);
Matrix3d I;
I.setIdentity();
if(vnormsq == 0)
return I;
Matrix3d R = rotationMatrix(v);
return (v*v.transpose() + (R.transpose()-I)*crossProductMatrix(v))/vnormsq;
}
const Matrix3d VectorMath::rotationMatrix(const Vector3d &axisAngle)
{
double theta = axisAngle.norm();
Vector3d thetahat = axisAngle/theta;
if(theta == 0)
thetahat.setZero();
Matrix3d result;
result.setIdentity();
result = cos(theta)*result + sin(theta)*crossProductMatrix(thetahat) + (1-cos(theta))*thetahat*thetahat.transpose();
return result;
}
const Vector3d VectorMath::axisAngle(const Matrix3d &rotationMatrix)
{
Matrix3d RminusI;
RminusI.setIdentity();
RminusI = rotationMatrix - RminusI;
JacobiSVD<MatrixXd> svd(RminusI, ComputeFullV);
//assert(fabs(svd.singularValues()[2]) < 1e-8);
Vector3d axis = svd.matrixV().col(2);
Vector3d testAxis = perpToAxis(axis);
Vector3d resultAxis = rotationMatrix*testAxis;
double theta = atan2(testAxis.cross(resultAxis).dot(axis), testAxis.dot(resultAxis));
return theta*axis;
}
/*
* deviator of a tensor
*/
static Matrix3d Deviator(Matrix3d M) {
Matrix3d eye;
eye.setIdentity();
eye *= M.trace() / 3.0;
return M - eye;
}
// Set up sparse linear system for Delayed Sparse Information Filter
void SysSPA2d::setupSparseDSIF(int newnode)
{
cout << "[SetupDSIF] at " << newnode << endl;
// set matrix sizes and clear
// assumes scales vars are all free
int nFree = nodes.size() - nFixed;
// long long t0, t1, t2, t3;
// t0 = utime();
// don't erase old stuff here, the delayed filter just grows in size
csp.setupBlockStructure(nFree,false); // initialize CSparse structures
// t1 = utime();
// loop over P2 constraints
for(size_t pi=0; pi<p2cons.size(); pi++)
{
Con2dP2 &con = p2cons[pi];
// don't consider old constraints
if (con.ndr < newnode && con.nd1 < newnode)
continue;
con.setJacobians(nodes);
Matrix3d J;
J.setIdentity();
Vector2d dRdth = nodes[con.ndr].dRdx.transpose() * con.tmean;
J(0,2) = dRdth(0);
J(1,2) = dRdth(1);
Matrix3d Jt = J.transpose();
Vector3d u;
u.head(2) = nodes[con.ndr].trans.head(2);
u(2) = nodes[con.ndr].arot;
Vector3d f;
f.head(2) = u.head(2) + nodes[con.ndr].w2n.transpose().block<2,2>(0,0) * con.tmean;
f(2) = u(2) + con.amean;
if (f(2) > M_PI) f(2) -= 2.0*M_PI;
if (f(2) < M_PI) f(2) += 2.0*M_PI;
cout << "[SetupDSIF] u = " << u.transpose() << endl;
cout << "[SetupDSIF] f = " << f.transpose() << endl;
cout << "[SetupDSIF] fo = " << nodes[con.nd1].trans.transpose() << endl;
// add in 4 blocks of A; actually just need upper triangular
int i0 = con.ndr-nFixed; // will be negative if fixed
int i1 = con.nd1-nFixed; // will be negative if fixed
if (i0 != i1-1) continue; // just sequential nodes for now
if (i0>=0)
{
Matrix<double,3,3> m = Jt*con.prec*J;
csp.addDiagBlock(m,i0);
}
if (i1>=0)
{
Matrix<double,3,3> m = con.prec;
csp.addDiagBlock(m,i1);
if (i0>=0)
{
Matrix<double,3,3> m2 = -con.prec * J;
if (i1 < i0)
{
m = m2.transpose();
csp.addOffdiagBlock(m,i1,i0);
}
else
{
csp.addOffdiagBlock(m2,i0,i1);
}
}
}
// add in 2 blocks of B
// Jt Gamma (J u - e)
Vector3d Juf = J*u - f;
if (Juf(2) > M_PI) Juf(2) -= 2.0*M_PI;
if (Juf(2) < M_PI) Juf(2) += 2.0*M_PI;
if (i0>=0)
{
csp.B.block<3,1>(i0*3,0) += Jt * con.prec * Juf;
}
if (i1>=0)
{
csp.B.block<3,1>(i1*3,0) -= con.prec * Juf;
}
} // finish P2 constraints
// t2 = utime();
csp.Bprev = csp.B; // save for next iteration
// set up sparse matrix structure from blocks
csp.setupCSstructure(1.0,true);
// t3 = utime();
// printf("\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
// (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
}
/// Run the Delayed Sparse Information Filter (Eustice et al.)
/// <newnode> is the index of the first new node added since the last iteration
/// <useCSparse> is true for sparse Cholesky.
void SysSPA2d::doDSIF(int newnode, bool useCSparse)
{
// number of nodes
int nnodes = nodes.size();
// set number of constraints
int ncons = p2cons.size();
// check for fixed frames
if (nFixed <= 0)
{
cout << "[doDSIF] No fixed frames" << endl;
return;
}
// check for newnode being ok
if (newnode >= nnodes)
{
cout << "[doDSIF] no new nodes to add" << endl;
return;
}
nFixed = 0;
for (int i=0; i<nnodes; i++)
{
Node2d &nd = nodes[i];
if (i >= nFixed)
nd.isFixed = false;
else
nd.isFixed = true;
nd.setTransform(); // set up world-to-node transform for cost calculation
nd.setDr(); // always use local angles
}
// initialize vars
double cost = calcCost();
if (verbose)
cout << " Initial squared cost: " << cost << " which is "
<< sqrt(cost/ncons) << " rms error" << endl;
if (newnode == 1)
{
// set up first system with node 0
csp.setupBlockStructure(1,false); // initialize CSparse structures
Matrix3d m;
m.setIdentity();
m = m*1000000;
csp.addDiagBlock(m,0);
Vector3d u;
u.head(2) = nodes[0].trans.head(2);
u(2) = nodes[0].arot;
csp.B.head(3) = u*1000000;
csp.Bprev = csp.B; // save for next iteration
cout << "[doDSIF] B = " << csp.B.transpose() << endl;
}
// set up and solve linear system
// NOTE: shouldn't need to redo all calcs in setupSys if we
// got here from a bad update
long long t0, t1, t2, t3;
t0 = utime();
if (useCSparse)
setupSparseDSIF(newnode); // set up sparse linear system
else
{}
#if 1
cout << "[doDSIF] B = " << csp.B.transpose() << endl;
csp.uncompress(A);
cout << "[doDSIF] A = " << endl << A << endl;
#endif
// cout << "[SPA] Solving...";
t1 = utime();
if (useCSparse)
{
bool ok = csp.doChol();
if (!ok)
cout << "[doDSIF] Sparse Cholesky failed!" << endl;
}
else
A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
t2 = utime();
// cout << "solved" << endl;
// get correct result vector
VectorXd &BB = useCSparse ? csp.B : B;
cout << "[doDSIF] RES = " << BB.transpose() << endl;
// update the frames
int ci = 0;
for(int i=0; i < nnodes; i++)
{
Node2d &nd = nodes[i];
if (nd.isFixed) continue; // not to be updated
nd.trans.head<2>() = BB.segment<2>(ci);
nd.arot = BB(ci+2);
nd.normArot();
nd.setTransform(); // set up projection matrix for cost calculation
nd.setDr(); // set rotational derivatives
ci += 3; // advance B index
}
// new cost
double newcost = calcCost();
if (verbose)
cout << " Updated squared cost: " << newcost << " which is "
<< sqrt(newcost/ncons) << " rms error" << endl;
t3 = utime();
}
bool scso3explog_tests()
{
double pi = 3.14159265;
vector<ScSO3> omegas;
omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, 0.0, 1.)));
omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, -1.0, 1.1)));
omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0., 1.1)));
omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.)));
omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.00001)));
omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0)));
omegas.push_back(ScSO3::exp(Vector4d(pi, 0, 0, 0.9)));
omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, 0.0,0))
*ScSO3::exp(Vector4d(pi, 0, 0,0.0))
*ScSO3::exp(Vector4d(-0.2, -0.5, -0.0,0)));
omegas.push_back(ScSO3::exp(Vector4d(0.3, 0.5, 0.1,0))
*ScSO3::exp(Vector4d(pi, 0, 0,0))
*ScSO3::exp(Vector4d(-0.3, -0.5, -0.1,0)));
bool failed = false;
for (size_t i=0; i<omegas.size(); ++i)
{
Matrix3d sR1 = omegas[i].matrix();
Matrix3d sR2 = ScSO3::exp(omegas[i].log()).matrix();
Matrix3d DiffR = sR1-sR2;
double nrm = DiffR.norm();
//// ToDO: Force ScSO3 to be more accurate!
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "ScSO3 - exp(log(ScSO3))" << endl;
cerr << "Test case: " << i << endl;
cerr << sR1 << endl;
cerr << omegas[i].log() << endl;
cerr << sR2 << endl;
cerr << DiffR <<endl;
cerr << endl;
failed = true;
}
}
for (size_t i=0; i<omegas.size(); ++i)
{
Vector3d p(1,2,4);
Matrix3d sR = omegas[i].matrix();
Vector3d res1 = omegas[i]*p;
Vector3d res2 = sR*p;
double nrm = (res1-res2).norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "Transform vector" << endl;
cerr << "Test case: " << i << endl;
cerr << (res1-res2) <<endl;
cerr << endl;
failed = true;
}
}
for (size_t i=0; i<omegas.size(); ++i)
{
Matrix3d q = omegas[i].matrix();
Matrix3d inv_q = omegas[i].inverse().matrix();
Matrix3d res = q*inv_q ;
Matrix3d I;
I.setIdentity();
double nrm = (res-I).norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "Inverse" << endl;
cerr << "Test case: " << i << endl;
cerr << (res-I) <<endl;
cerr << endl;
failed = true;
}
Matrix3d R = omegas[i].rotationMatrix();
cerr << R*R.transpose() << endl;
}
return failed;
}
void Omega3::AcquisitionTask::run(){
while(1){
//! qDebug()<< "try to open the first available device";
if (drdOpen () < 0) {
//qDebug()<<"no device available...";
//dhdSleep (2.0);
omegaDetected = false;
}
else{
omegaDetected = true;
}
if(omegaDetected){
break;
}
sleep(1);
}
done = 0;
double p[DHD_MAX_DOF];
double r[3][3];
double v[DHD_MAX_DOF];
double f[DHD_MAX_DOF];
double normf, normt;
double t0 = dhdGetTime ();
double t1 = t0;
bool base = false;
bool wrist = false;
bool grip = false;
//! Eigen objects (mapped to the arrays above)
Map<Vector3d> position(&p[0], 3);
Map<Vector3d> force (&f[0], 3);
Map<Vector3d> torque (&f[3], 3);
Map<Vector3d> velpos (&v[0], 3);
Map<Vector3d> velrot (&v[3], 3);
Matrix3d center;
Matrix3d rotation;
//! center of workspace
center.setIdentity (); // rotation (matrix)
double nullPose[DHD_MAX_DOF] = { 0.0, 0.0, 0.0, // translation
0.0, 0.0, 0.0, // rotation (joint angles)
0.0 }; // gripper
//! print out device identifier
if (!drdIsSupported ()) {
dhdSleep (2.0);
drdClose ();
}
//! perform auto-initialization
if (!drdIsInitialized () && drdAutoInit () < 0) {
qDebug()<<"error: auto-initialization failed"<<dhdErrorGetLastStr ();
dhdSleep (2.0);
}
else if (drdStart () < 0) {
printf ("error: regulation thread failed to start (%s)\n", dhdErrorGetLastStr ());
dhdSleep (2.0);
}
//! move to center
drdMoveTo (nullPose);
// request a null force (only gravity compensation will be applied)
// this will only apply to unregulated axis
for (int i=0; i<DHD_MAX_DOF; i++) f[i] = 0.0;
drdSetForceAndTorqueAndGripperForce (f);
// disable all axis regulation (but leave regulation thread running)
drdRegulatePos (base);
drdRegulateRot (wrist);
drdRegulateGrip (grip);
//! save current pos
drdGetPositionAndOrientation (p, r);
for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);
this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
long long cpt = 0;
// loop while the button is not pushed
while (!done) {
// synchronize with regulation thread
drdWaitForTick ();
// get position/orientation/gripper and update Eigen rotation matrix
drdGetPositionAndOrientation (p, r);
for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);
if(cpt%31==0){
this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
}
else{
cpt += 1;
}
// get position/orientation/gripper velocity
drdGetVelocity (v);
// compute base centering force
force = - KP * position;
// compute wrist centering torque
AngleAxisd deltaRotation (rotation.transpose() * center);
torque = rotation * (KR * deltaRotation.angle() * deltaRotation.axis());
// compute gripper centering force
f[6] = - KG * (p[6] - 0.015);
// scale force to a pre-defined ceiling
if ((normf = force.norm()) > MAXF) force *= MAXF/normf;
// scale torque to a pre-defined ceiling
if ((normt = torque.norm()) > MAXT) torque *= MAXT/normt;
// scale gripper force to a pre-defined ceiling
if (f[6] > MAXG) f[6] = MAXG;
if (f[6] < -MAXG) f[6] = -MAXG;
// add damping
force -= KVP * velpos;
torque -= KWR * velrot;
f[6] -= KVG * v[6];
// apply centering force with damping
if (drdSetForceAndTorqueAndGripperForce (f, -1) < DHD_NO_ERROR) {
printf ("error: cannot set force (%s)\n", dhdErrorGetLastStr ());
done = 1;
}
// local loop refresh rate
//count++;
// display refresh rate and position at 10Hz
t1 = drdGetTime ();
if ((t1-t0) > REFRESH_INTERVAL) {
// retrieve/compute information to display
// double freq = (double)count/(t1-t0)*1e-3;
// count = 0;
// t0 = t1;
// qDebug()<<freq<<cpt;
if (dhdGetButtonMask()) {
qDebug()<<"stop";
dhdClose ();
done = 1;
}
}
//qDebug()<<cpt<<t1;
//dhdSleep (0.1);
}
}
int main(int argc, char** argv) {
CommandArgs arg;
int nlandmarks;
int nSegments;
int simSteps;
double worldSize;
bool hasOdom;
bool hasPoseSensor;
bool hasPointSensor;
bool hasPointBearingSensor;
bool hasSegmentSensor;
bool hasCompass;
bool hasGPS;
int segmentGridSize;
double minSegmentLenght, maxSegmentLenght;
std::string outputFilename;
arg.param("simSteps", simSteps, 100, "number of simulation steps");
arg.param("nLandmarks", nlandmarks, 1000, "number of landmarks");
arg.param("nSegments", nSegments, 1000, "number of segments");
arg.param("segmentGridSize", segmentGridSize, 50, "number of cells of the grid where to align the segments");
arg.param("minSegmentLenght", minSegmentLenght, 0.5, "minimal lenght of a segment in the world");
arg.param("maxSegmentLenght", maxSegmentLenght, 3, "maximal lenght of a segment in the world");
arg.param("worldSize", worldSize, 25.0, "size of the world");
arg.param("hasOdom", hasOdom, false, "the robot has an odometry" );
arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" );
arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" );
arg.param("hasPoseSensor", hasPoseSensor, false, "the robot has a pose sensor" );
arg.param("hasCompass", hasCompass, false, "the robot has a compass");
arg.param("hasGPS", hasGPS, false, "the robot has a GPS");
arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" );
arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true);
arg.parseArgs(argc, argv);
std::tr1::ranlux_base_01 generator;
OptimizableGraph graph;
World world(&graph);
for (int i=0; i<nlandmarks; i++){
WorldObjectPointXY * landmark = new WorldObjectPointXY;
double x = sampleUniform(-.5,.5, &generator)*worldSize;
double y = sampleUniform(-.5,.5, &generator)*worldSize;
landmark->vertex()->setEstimate(Vector2d(x,y));
world.addWorldObject(landmark);
}
cerr << "nSegments = " << nSegments << endl;
for (int i=0; i<nSegments; i++){
WorldObjectSegment2D * segment = new WorldObjectSegment2D;
int ix = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
int iy = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
int ith = sampleUniform(0,3, &generator);
double th= (M_PI/2)*ith;
th=atan2(sin(th),cos(th));
double xc = ix*(worldSize/segmentGridSize);
double yc = iy*(worldSize/segmentGridSize);
double l2 = sampleUniform(minSegmentLenght, maxSegmentLenght, &generator);
double x1 = xc + cos(th)*l2;
double y1 = yc + sin(th)*l2;
double x2 = xc - cos(th)*l2;
double y2 = yc - sin(th)*l2;
segment->vertex()->setEstimateP1(Vector2d(x1,y1));
segment->vertex()->setEstimateP2(Vector2d(x2,y2));
world.addWorldObject(segment);
}
Robot2D robot(&world, "myRobot");
world.addRobot(&robot);
stringstream ss;
ss << "-ws" << worldSize;
ss << "-nl" << nlandmarks;
ss << "-steps" << simSteps;
if (hasOdom) {
SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry");
robot.addSensor(odometrySensor);
Matrix3d odomInfo = odometrySensor->information();
odomInfo.setIdentity();
odomInfo*=100;
odomInfo(2,2)=1000;
odometrySensor->setInformation(odomInfo);
ss << "-odom";
}
if (hasPoseSensor) {
SensorPose2D* poseSensor = new SensorPose2D("poseSensor");
robot.addSensor(poseSensor);
Matrix3d poseInfo = poseSensor->information();
poseInfo.setIdentity();
poseInfo*=100;
poseInfo(2,2)=1000;
poseSensor->setInformation(poseInfo);
ss << "-pose";
}
if (hasPointSensor) {
SensorPointXY* pointSensor = new SensorPointXY("pointSensor");
robot.addSensor(pointSensor);
Matrix2d pointInfo = pointSensor->information();
pointInfo.setIdentity();
pointInfo*=100;
pointSensor->setInformation(pointInfo);
ss << "-pointXY";
}
if (hasPointBearingSensor) {
SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor");
robot.addSensor(bearingSensor);
bearingSensor->setInformation(bearingSensor->information()*1000);
ss << "-pointBearing";
}
if (hasSegmentSensor) {
SensorSegment2D* segmentSensor = new SensorSegment2D("segmentSensorSensor");
segmentSensor->setMaxRange(3);
segmentSensor->setMinRange(.1);
robot.addSensor(segmentSensor);
segmentSensor->setInformation(segmentSensor->information()*1000);
SensorSegment2DLine* segmentSensorLine = new SensorSegment2DLine("segmentSensorSensorLine");
segmentSensorLine->setMaxRange(3);
segmentSensorLine->setMinRange(.1);
robot.addSensor(segmentSensorLine);
Matrix2d m=segmentSensorLine->information();
m=m*1000;
m(0,0)*=10;
segmentSensorLine->setInformation(m);
SensorSegment2DPointLine* segmentSensorPointLine = new SensorSegment2DPointLine("segmentSensorSensorPointLine");
segmentSensorPointLine->setMaxRange(3);
segmentSensorPointLine->setMinRange(.1);
robot.addSensor(segmentSensorPointLine);
Matrix3d m3=segmentSensorPointLine->information();
m3=m3*1000;
m3(3,3)*=10;
segmentSensorPointLine->setInformation(m3);
ss << "-segment2d";
}
robot.move(SE2());
double pStraight=0.7;
SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.));
double pLeft=0.15;
SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2));
//double pRight=0.15;
SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2));
for (int i=0; i<simSteps; i++){
cerr << "m";
SE2 move;
SE2 pose=robot.pose();
double dtheta=-100;
Vector2d dt;
if (pose.translation().x() < -.5*worldSize){
dtheta = 0;
}
if (pose.translation().x() > .5*worldSize){
dtheta = -M_PI;
}
if (pose.translation().y() < -.5*worldSize){
dtheta = M_PI/2;
}
if (pose.translation().y() > .5*worldSize){
dtheta = -M_PI/2;
}
if (dtheta< -M_PI) {
// select a random move of the robot
double sampled=sampleUniform(0.,1.,&generator);
if (sampled<pStraight)
move=moveStraight;
else if (sampled<pStraight+pLeft)
move=moveLeft;
else
move=moveRight;
} else {
double mTheta=dtheta-pose.rotation().angle();
move.setRotation(Rotation2Dd(mTheta));
if (move.rotation().angle()<std::numeric_limits<double>::epsilon()){
move.setTranslation(Vector2d(1., 0.));
}
}
robot.relativeMove(move);
// do a sense
cerr << "s";
robot.sense();
}
//string fname=outputFilename + ss.str() + ".g2o";
ofstream testStream(outputFilename.c_str());
graph.save(testStream);
return 0;
}
bool se2explog_tests()
{
double pi = 3.14159265;
vector<SE2> omegas;
omegas.push_back(SE2(SO2(0.0),Vector2d(0,0)));
omegas.push_back(SE2(SO2(0.2),Vector2d(10,0)));
omegas.push_back(SE2(SO2(0.),Vector2d(0,100)));
omegas.push_back(SE2(SO2(-1.),Vector2d(20,-1)));
omegas.push_back(SE2(SO2(0.00001),Vector2d(-0.00000001,0.0000000001)));
omegas.push_back(SE2(SO2(0.2),Vector2d(0,0))
*SE2(SO2(pi),Vector2d(0,0))
*SE2(SO2(-0.2),Vector2d(0,0)));
omegas.push_back(SE2(SO2(0.3),Vector2d(2,0))
*SE2(SO2(pi),Vector2d(0,0))
*SE2(SO2(-0.3),Vector2d(0,6)));
bool failed = false;
for (size_t i=0; i<omegas.size(); ++i)
{
Matrix3d R1 = omegas[i].matrix();
Matrix3d R2 = SE2::exp(omegas[i].log()).matrix();
Matrix3d DiffR = R1-R2;
double nrm = DiffR.norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "SE2 - exp(log(SE2))" << endl;
cerr << "Test case: " << i << endl;
cerr << DiffR <<endl;
cerr << endl;
failed = true;
}
}
for (size_t i=0; i<omegas.size(); ++i)
{
Vector2d p(1,2);
Matrix3d T = omegas[i].matrix();
Vector2d res1 = omegas[i]*p;
Vector2d res2 = T.topLeftCorner<2,2>()*p + T.topRightCorner<2,1>();
double nrm = (res1-res2).norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "Transform vector" << endl;
cerr << "Test case: " << i << endl;
cerr << (res1-res2) <<endl;
cerr << endl;
failed = true;
}
}
for (size_t i=0; i<omegas.size(); ++i)
{
Matrix3d q = omegas[i].matrix();
Matrix3d inv_q = omegas[i].inverse().matrix();
Matrix3d res = q*inv_q ;
Matrix3d I;
I.setIdentity();
double nrm = (res-I).norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "Inverse" << endl;
cerr << "Test case: " << i << endl;
cerr << (res-I) <<endl;
cerr << endl;
failed = true;
}
}
return failed;
}
void CFullRelPose::writeEdge(std::ostream & toroGraphFile, const bool b2d) const
{
if(IS_DEBUG) CHECK(!hasPosition(), "Node is disconnected?? Or too bad");
const double dLength = length();
//double dVar = sqr(dLength/4);// scale.scaleVarHacked();
double dVar = variance();
dVar = std::max<double>(1e-5, dVar);
normalisedPose.scale(dLength).write(toroGraphFile, b2d);
Vector3d t, x_axis; x_axis.setZero();
normalisedPose.t.asVector(t);
if(b2d)
{
//double inf_ff, inf_fs, inf_ss, inf_rr, inf_fr, inf_sr;
Vector2d t2d(t(0), t(2));
Vector2d t2d_perp(t(2), -t(0));
Matrix2d Q, LAMBDA;
LAMBDA.setZero();
Q << t2d, t2d_perp;
LAMBDA(0,0) = dVar; //dVar may be zero
LAMBDA(1,1) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());
CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")
const Matrix2d C = Q * LAMBDA * Q.transpose();
const Matrix2d & I = C.inverse();
//cout << I << endl;
//THROW("Not complete...")
double dRotInf = 1.0/sqr(normalisedPose.SD.relOrientationSD());
toroGraphFile << I(0,0) << ' ' << I(1,0) << ' ' << I(1,1) << ' ' << dRotInf << " 0 0";
return;
}
if(t(0) < 0.9) //check t isn't x axis aligned (x is arbitrary)
x_axis(0) = 1;
else
x_axis(1) = 1;
Matrix3d Q;
if(t.sum() == 0) //Pure rotation. Should be arbitrary
{
Q.setIdentity();
}
else
{
Vector3d tperp1 = t.cross(x_axis);
tperp1.normalize();
Vector3d tperp2 = tperp1.cross(t);
Q << t, tperp1, tperp2;
}
CHECK(isnan(Q.sum()), "writeEdge: nan")
Matrix3d LAMBDA; LAMBDA.setZero();
LAMBDA(0,0) = dVar; //dVar may be zero
LAMBDA(1,1) = LAMBDA(2,2) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());
CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")
const Matrix3d C = Q * LAMBDA * Q.transpose();
//std::cout << "Covariance: " << std::endl << C << std::endl;
CHECK(isnan(C.sum()), "writeEdge: nan")
Matrix3d Crpy; Crpy.setZero();
Crpy.diagonal().setConstant(sqr(normalisedPose.SD.relOrientationSD()));
Matrix<double, 6, 6> Cfull; Cfull << C, Matrix3d::Zero(), Matrix3d::Zero(), Crpy;
CHECK(isnan(Cfull.sum()), "writeEdge: nan")
if(Cfull.trace() < 0.0001)
{
std::cout << "Warning: Covariance matrix near-singular, adjusting diagonal\n";
Cfull.diagonal().array() += 0.0001;
}
const Matrix<double, 6, 6> & Ifull = Cfull.inverse();
//std::cout << "Information: " << std::endl << Ifull << std::endl;
//cout << "Warning: Not inverting information matrix\n"; Yes the information mat does work a little better.
//Possibly ok for 1 edge...if(IS_DEBUG) CHECK(Cfull.determinant()<0.0001, "CFullRelPose::writeEdge: Singular covariance matrix");
CHECK(isnan(Ifull.sum()), "writeEdge: nan")
for (int nRow = 0; nRow <6; nRow++)
{
for(int nCol = nRow; nCol < 6; nCol++)
toroGraphFile << Ifull(nRow, nCol) << ' ';
}
}
bool so3explog_tests()
{
vector<SO3> omegas;
omegas.push_back(SO3(Quaterniond(0.1e-11, 0., 1., 0.)));
omegas.push_back(SO3(Quaterniond(-1,0.00001,0.0,0.0)));
omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, 0.0)));
omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, -1.0)));
omegas.push_back(SO3::exp(Vector3d(0., 0., 0.)));
omegas.push_back(SO3::exp(Vector3d(0., 0., 0.00001)));
omegas.push_back(SO3::exp(Vector3d(M_PI, 0, 0)));
omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, 0.0))
*SO3::exp(Vector3d(M_PI, 0, 0))
*SO3::exp(Vector3d(-0.2, -0.5, -0.0)));
omegas.push_back(SO3::exp(Vector3d(0.3, 0.5, 0.1))
*SO3::exp(Vector3d(M_PI, 0, 0))
*SO3::exp(Vector3d(-0.3, -0.5, -0.1)));
bool failed = false;
for (size_t i=0; i<omegas.size(); ++i)
{
Matrix3d R1 = omegas[i].matrix();
double theta;
Matrix3d R2 = SO3::exp(SO3::logAndTheta(omegas[i],&theta)).matrix();
Matrix3d DiffR = R1-R2;
double nrm = DiffR.norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "SO3 - exp(log(SO3))" << endl;
cerr << "Test case: " << i << endl;
cerr << DiffR <<endl;
cerr << endl;
failed = true;
}
if (theta>M_PI || theta<-M_PI)
{
cerr << "log theta not in [-pi,pi]" << endl;
cerr << "Test case: " << i << endl;
cerr << theta <<endl;
cerr << endl;
failed = true;
}
}
for (size_t i=0; i<omegas.size(); ++i)
{
Vector3d p(1,2,4);
Matrix3d sR = omegas[i].matrix();
Vector3d res1 = omegas[i]*p;
Vector3d res2 = sR*p;
double nrm = (res1-res2).norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "Transform vector" << endl;
cerr << "Test case: " << i << endl;
cerr << (res1-res2) <<endl;
cerr << endl;
failed = true;
}
}
for (size_t i=0; i<omegas.size(); ++i)
{
Matrix3d q = omegas[i].matrix();
Matrix3d inv_q = omegas[i].inverse().matrix();
Matrix3d res = q*inv_q ;
Matrix3d I;
I.setIdentity();
double nrm = (res-I).norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "Inverse" << endl;
cerr << "Test case: " << i << endl;
cerr << (res-I) <<endl;
cerr << endl;
failed = true;
}
}
return failed;
}
int main(int argc, char** argv) {
CommandArgs arg;
int nlandmarks;
int simSteps;
double worldSize;
bool hasOdom;
bool hasPoseSensor;
bool hasPointSensor;
bool hasPointBearingSensor;
bool hasSegmentSensor;
bool hasCompass;
bool hasGPS;
std::string outputFilename;
arg.param("simSteps", simSteps, 100, "number of simulation steps");
arg.param("worldSize", worldSize, 25.0, "size of the world");
arg.param("hasOdom", hasOdom, false, "the robot has an odometry" );
arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" );
arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" );
arg.param("hasPoseSensor", hasPoseSensor, false, "the robot has a pose sensor" );
arg.param("hasCompass", hasCompass, false, "the robot has a compass");
arg.param("hasGPS", hasGPS, false, "the robot has a GPS");
arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" );
arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true);
arg.parseArgs(argc, argv);
std::tr1::ranlux_base_01 generator;
OptimizableGraph graph;
World world(&graph);
for (int i=0; i<nlandmarks; i++){
WorldObjectPointXY * landmark = new WorldObjectPointXY;
double x = sampleUniform(-.5,.5, &generator)*worldSize;
double y = sampleUniform(-.5,.5, &generator)*worldSize;
landmark->vertex()->setEstimate(Vector2d(x,y));
world.addWorldObject(landmark);
}
Robot2D robot(&world, "myRobot");
world.addRobot(&robot);
stringstream ss;
ss << "-ws" << worldSize;
ss << "-nl" << nlandmarks;
ss << "-steps" << simSteps;
if (hasOdom) {
SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry");
robot.addSensor(odometrySensor);
Matrix3d odomInfo = odometrySensor->information();
odomInfo.setIdentity();
odomInfo*=100;
odomInfo(2,2)=1000;
odometrySensor->setInformation(odomInfo);
ss << "-odom";
}
if (hasPoseSensor) {
SensorPose2D* poseSensor = new SensorPose2D("poseSensor");
robot.addSensor(poseSensor);
Matrix3d poseInfo = poseSensor->information();
poseInfo.setIdentity();
poseInfo*=100;
poseInfo(2,2)=1000;
poseSensor->setInformation(poseInfo);
ss << "-pose";
}
if (hasPointSensor) {
SensorPointXY* pointSensor = new SensorPointXY("pointSensor");
robot.addSensor(pointSensor);
Matrix2d pointInfo = pointSensor->information();
pointInfo.setIdentity();
pointInfo*=100;
pointSensor->setInformation(pointInfo);
ss << "-pointXY";
}
if (hasPointBearingSensor) {
SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor");
robot.addSensor(bearingSensor);
bearingSensor->setInformation(bearingSensor->information()*1000);
ss << "-pointBearing";
}
robot.move(SE2());
double pStraight=0.7;
SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.));
double pLeft=0.15;
SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2));
//double pRight=0.15;
SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2));
for (int i=0; i<simSteps; i++){
cerr << "m";
SE2 move;
SE2 pose=robot.pose();
double dtheta=-100;
Vector2d dt;
if (pose.translation().x() < -.5*worldSize){
dtheta = 0;
}
if (pose.translation().x() > .5*worldSize){
dtheta = -M_PI;
}
if (pose.translation().y() < -.5*worldSize){
dtheta = M_PI/2;
}
if (pose.translation().y() > .5*worldSize){
dtheta = -M_PI/2;
}
if (dtheta< -M_PI) {
// select a random move of the robot
double sampled=sampleUniform(0.,1.,&generator);
if (sampled<pStraight)
move=moveStraight;
else if (sampled<pStraight+pLeft)
move=moveLeft;
else
move=moveRight;
} else {
double mTheta=dtheta-pose.rotation().angle();
move.setRotation(Rotation2Dd(mTheta));
if (move.rotation().angle()<std::numeric_limits<double>::epsilon()){
move.setTranslation(Vector2d(1., 0.));
}
}
robot.relativeMove(move);
// do a sense
cerr << "s";
robot.sense();
}
//string fname=outputFilename + ss.str() + ".g2o";
ofstream testStream(outputFilename.c_str());
graph.save(testStream);
}