int main() {
using V = std::vector<float>;
using SoaV = std::array<V,3>;
using SoaN = std::tuple<V,V,V>;
SoaV s = {V(10),V(10),V(10)};
SoaN s2{V(10),V(10),V(10)};
auto v0 = a2v(s, 3);
auto v2 = t2v(s2, 5);
v0.x=-9.; v2.y=v0.x;
std::cout << s[0][3] << ' ' << std::get<1>(s2)[5] << std::endl;
Soa<Vect<float>, float,float,float,int> soaI(30);
std::cout << soaI.size() << std::endl;
auto vk = soaI[23];
vk.x=3.14;
soaI[11] = v0;
std::cout << std::get<0>(soaI.data)[23] << ' ' << std::get<0>(soaI.data)[11] << std::endl;
return v0.x*v2.y;
}
void Aligner::serialize(boss::ObjectData &data, boss::IdContext &context) {
boss::Identifiable::serialize(data, context);
data.setInt("outerIterations", outerIterations());
data.setInt("innerIterations", innerIterations());
t2v(_referenceSensorOffset).toBOSS(data, "referenceSensorOffset");
t2v(_currentSensorOffset).toBOSS(data, "currentSensorOffset");
PointProjector *projector = dynamic_cast<PointProjector*>(_projector);
if(!projector) {
throw std::runtime_error("Impossible to convert pwn::PointProjector to pwn_boss::PointProjector");
}
data.setPointer("projector", projector);
Linearizer *linearizer = dynamic_cast<Linearizer*>(_linearizer);
if(!linearizer) {
throw std::runtime_error("Impossible to convert pwn::Linearizer to pwn_boss::Linearizer");
}
data.setPointer("linearizer", linearizer);
CorrespondenceFinder *correspondenceFinder = dynamic_cast<CorrespondenceFinder*>(_correspondenceFinder);
if(!correspondenceFinder) {
throw std::runtime_error("Impossible to convert pwn::CorrespondenceFinder to pwn_boss::CorrespondenceFinder");
}
data.setPointer("correspondenceFinder", correspondenceFinder);
}
void Aligner::_computeStatistics(Vector6f &mean, Matrix6f &Omega,
float &translationalRatio, float &rotationalRatio) const {
typedef SigmaPoint<Vector6f> SigmaPoint;
typedef std::vector<SigmaPoint, Eigen::aligned_allocator<SigmaPoint> > SigmaPointVector;
// Output init
Vector6f b;
Matrix6f H;
b.setZero();
H.setZero();
translationalRatio = std::numeric_limits<float>::max();
rotationalRatio = std::numeric_limits<float>::max();
Eigen::Isometry3f invT = _T.inverse();
invT.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
_linearizer->setT(invT);
_linearizer->update();
H += _linearizer->H() + Matrix6f::Identity();
b += _linearizer->b();
JacobiSVD<Matrix6f> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
Matrix6f localSigma = svd.solve(Matrix6f::Identity());
SigmaPointVector sigmaPoints;
Vector6f localMean = Vector6f::Zero();
sampleUnscented(sigmaPoints, localMean, localSigma);
Eigen::Isometry3f dT = _T; // Transform from current to reference
// Remap each of the sigma points to their original position
//#pragma omp parallel
for (size_t i = 0; i < sigmaPoints.size(); i++) {
SigmaPoint &p = sigmaPoints[i];
p._sample = t2v(dT * v2t(p._sample).inverse());
}
// Reconstruct the gaussian
reconstructGaussian(mean, localSigma, sigmaPoints);
// Compute the information matrix from the covariance
Omega = localSigma.inverse();
// Have a look at the svd of the rotational and the translational part;
JacobiSVD<Matrix3f> partialSVD;
partialSVD.compute(Omega.block<3, 3>(0, 0));
translationalRatio = partialSVD.singularValues()(0) / partialSVD.singularValues()(2);
partialSVD.compute(Omega.block<3, 3>(3, 3));
rotationalRatio = partialSVD.singularValues()(0) / partialSVD.singularValues()(2);
}
// note: CONCATENATE does side-effect on transf
RobotPosition * concatenate(RobotPosition * step, Eigen::Matrix3d& transf){
Eigen::Vector3d buff_vect;
double rstep[3];
step->getEstimateData(rstep);
buff_vect << rstep[0], rstep[1], rstep[2];
transf = transf * v2t(buff_vect);
buff_vect = t2v(transf);
RobotPosition * ret = new RobotPosition(buff_vect[0], buff_vect[1], buff_vect[2]);
for(unsigned int obsnum = 0; obsnum<step->observations.size(); obsnum++){
ret->addObs(step->observations[obsnum].bearing);
}
return ret;
}
void posesToTransformations(std::vector<RobotPosition*> *poses, std::vector<RobotPosition*> *transformations){
transformations->clear();
Eigen::Matrix3d M1;
Eigen::Matrix3d M2;
M1 << 1, 0, 0,
0, 1, 0,
0, 0, 1;
for(unsigned int p=0; p<poses->size(); p++){
M2 = r2t((*poses)[p]);
Eigen::Vector3d t = t2v(M1.inverse()*M2);
RobotPosition * rototransl = new RobotPosition(t[0], t[1], t[2]);
for(unsigned int o=0; o<(*poses)[p]->observations.size(); o++){
rototransl->addObs((*poses)[p]->observations[o].bearing);
}
transformations->push_back(rototransl);
M1=M2;
}
}
Vector6f SE3OffsetErrorFunction::operator()(const Vector6f& x) const {
return t2v(_A*v2t(x)*_B);
}
void Aligner::align() {
assert(_projector && "Aligner: missing _projector");
assert(_linearizer && "Aligner: missing _linearizer");
assert(_correspondenceFinder && "Aligner: missing _correspondenceFinder");
assert(_referenceCloud && "Aligner: missing _referenceCloud");
assert(_currentCloud && "Aligner: missing _currentCloud");
struct timeval tvStart, tvEnd;
gettimeofday(&tvStart, 0);
// The current points are seen from the frame of the sensor
_projector->setTransform(_currentSensorOffset);
_projector->project(_correspondenceFinder->currentIndexImage(),
_correspondenceFinder->currentDepthImage(),
_currentCloud->points());
_T = _initialGuess;
for(int i = 0; i < _outerIterations; i++) {
/************************************************************************
* Correspondence Computation *
************************************************************************/
// Compute the indices of the current scene from the point of view of the sensor
_T.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
_projector->setTransform(_T * _referenceSensorOffset);
_projector->project(_correspondenceFinder->referenceIndexImage(),
_correspondenceFinder->referenceDepthImage(),
_referenceCloud->points());
// Correspondences computation.
_correspondenceFinder->compute(*_referenceCloud, *_currentCloud, _T.inverse());
/************************************************************************
* Alignment *
************************************************************************/
Eigen::Isometry3f invT = _T.inverse();
for(int k = 0; k < _innerIterations; k++) {
invT.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
Matrix6f H;
Vector6f b;
_linearizer->setT(invT);
_linearizer->update();
H = _linearizer->H() + Matrix6f::Identity();
b = _linearizer->b();
H += Matrix6f::Identity() * 1000.0f;
// Add the priors
for(size_t j = 0; j < _priors.size(); j++) {
const SE3Prior *prior = _priors[j];
Vector6f priorError = prior->error(invT);
Matrix6f priorJacobian = prior->jacobian(invT);
Matrix6f priorInformationRemapped = prior->errorInformation(invT);
Matrix6f Hp = priorJacobian.transpose() * priorInformationRemapped * priorJacobian;
Vector6f bp = priorJacobian.transpose() * priorInformationRemapped * priorError;
H += Hp;
b += bp;
}
Vector6f dx = H.ldlt().solve(-b);
Eigen::Isometry3f dT = v2t(dx);
invT = dT * invT;
}
_T = invT.inverse();
_T = v2t(t2v(_T));
_T.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
}
gettimeofday(&tvEnd, 0);
double tStart = tvStart.tv_sec * 1000.0f + tvStart.tv_usec * 0.001f;
double tEnd = tvEnd.tv_sec * 1000.0f + tvEnd.tv_usec * 0.001f;
_totalTime = tEnd - tStart;
_error = _linearizer->error();
_inliers = _linearizer->inliers();
_computeStatistics(_mean, _omega, _translationalEigenRatio, _rotationalEigenRatio);
if (_rotationalEigenRatio > _rotationalMinEigenRatio ||
_translationalEigenRatio > _translationalMinEigenRatio) {
if (_debug) {
cerr << endl;
cerr << "************** WARNING SOLUTION MIGHT BE INVALID (eigenratio failure) **************" << endl;
cerr << "tr: " << _translationalEigenRatio << " rr: " << _rotationalEigenRatio << endl;
cerr << "************************************************************************************" << endl;
}
}
else {
if (_debug) {
cerr << "************** I FOUND SOLUTION VALID SOLUTION (eigenratio ok) *******************" << endl;
cerr << "tr: " << _translationalEigenRatio << " rr: " << _rotationalEigenRatio << endl;
cerr << "************************************************************************************" << endl;
}
}
if (_debug) {
cout << "Solution statistics in (t, mq): " << endl;
cout << "mean: " << _mean.transpose() << endl;
cout << "Omega: " << endl;
cout << _omega << endl;
}
}
