void computeSensorOffsetAndK(Eigen::Isometry3f &sensorOffset, Eigen::Matrix3f &cameraMatrix, ParameterCamera *cameraParam, int reduction) {
sensorOffset = Isometry3f::Identity();
cameraMatrix.setZero();
int cmax = 4;
int rmax = 3;
for (int c=0; c<cmax; c++){
for (int r=0; r<rmax; r++){
sensorOffset.matrix()(r, c) = cameraParam->offset()(r, c);
if (c<3)
cameraMatrix(r,c) = cameraParam->Kcam()(r, c);
}
}
sensorOffset.translation() = Vector3f(0.15f, 0.0f, 0.05f);
Quaternionf quat = Quaternionf(0.5, -0.5, 0.5, -0.5);
sensorOffset.linear() = quat.toRotationMatrix();
sensorOffset.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
float scale = 1./reduction;
cameraMatrix *= scale;
cameraMatrix(2,2) = 1;
}
bool extractRelativePrior(Eigen::Isometry3f& priorMean, Matrix6f& priorInfo,
const DrawableFrame* reference, const DrawableFrame* current){
VertexSE3* referenceVertex =reference->_vertex;
VertexSE3* currentVertex =current->_vertex;
bool priorFound = false;
priorInfo.setZero();
for (HyperGraph::EdgeSet::const_iterator it=referenceVertex->edges().begin();
it!= referenceVertex->edges().end(); it++){
const EdgeSE3* e = dynamic_cast<const EdgeSE3*>(*it);
if (e->vertex(0)==referenceVertex && e->vertex(1) == currentVertex){
priorFound=true;
for (int c=0; c<6; c++)
for (int r=0; r<6; r++)
priorInfo(r,c) = e->information()(r,c);
for(int c=0; c<4; c++)
for(int r=0; r<3; r++)
priorMean.matrix()(r,c) = e->measurement().matrix()(r,c);
priorMean.matrix().row(3) << 0,0,0,1;
}
}
return priorFound;
}
void PinholePointProjector::_updateMatrices() {
Eigen::Isometry3f t =_transform.inverse();
t.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
_iK = _cameraMatrix.inverse();
_KR = _cameraMatrix * t.linear();
_Kt = _cameraMatrix * t.translation();
_iKR = _transform.linear() * _iK;
_iKt = _transform.translation();
_KRt.setIdentity();
_iKRt.setIdentity();
_KRt.block<3, 3>(0, 0) = _KR;
_KRt.block<3, 1>(0, 3) = _Kt;
_iKRt.block<3, 3>(0, 0) = _iKR;
_iKRt.block<3, 1>(0, 3) = _iKt;
}
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);
}
void Aligner::align() {
struct timeval tvStart, tvEnd;
gettimeofday(&tvStart,0);
if (! _projector || !_linearizer || !_correspondenceGenerator){
cerr << "I do nothing since you did not set all required algorithms" << endl;
return;
}
// the current points are seen from the frame of the sensor
_projector->setTransform(_sensorOffset);
_projector->project(_correspondenceGenerator->currentIndexImage(),
_correspondenceGenerator->currentDepthImage(),
_currentScene->points());
_T = _initialGuess;
for(int i = 0; i < _outerIterations; i++) {
//cout << "********************* Iteration " << i << " *********************" << endl;
/************************************************************************
* Correspondence Computation *
************************************************************************/
//cout << "Computing correspondences...";
// compute the indices of the current scene from the point of view of the sensor
_projector->setTransform(_T*_sensorOffset);
_projector->project(_correspondenceGenerator->referenceIndexImage(),
_correspondenceGenerator->referenceDepthImage(),
_referenceScene->points());
// Correspondences computation.
_correspondenceGenerator->compute(*_referenceScene, *_currentScene, _T.inverse());
//cout << " done." << endl;
//cout << "# inliers found: " << _correspondenceGenerator->numCorrespondences() << endl;
/************************************************************************
* Alignment *
************************************************************************/
Eigen::Isometry3f invT = _T.inverse();
for (int k = 0; k < _innerIterations; k++) {
invT.matrix().block<1, 4>(3, 0) << 0, 0, 0, 1;
Matrix6f H;
Vector6f b;
_linearizer->setT(invT);
_linearizer->update();
H = _linearizer->H() + Matrix6f::Identity() * 10.0f;
b = _linearizer->b();
// add the priors
for (size_t i=0; i<_priors.size(); i++){
const SE3Prior* prior = _priors[i];
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.matrix().block<1, 4>(3, 0) << 0, 0, 0, 1;
}
gettimeofday(&tvEnd, 0);
double tStart = tvStart.tv_sec*1000+tvStart.tv_usec*0.001;
double tEnd = tvEnd.tv_sec*1000+tvEnd.tv_usec*0.001;
_totalTime = tEnd - tStart;
_error = _linearizer->error();
_inliers = _linearizer->inliers();
}
void ViewerState::load(const std::string& filename){
clear();
listWidget->clear();
graph->clear();
graph->load(filename.c_str());
vector<int> vertexIds(graph->vertices().size());
int k=0;
for (OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
vertexIds[k++] = (it->first);
}
sort(vertexIds.begin(), vertexIds.end());
listAssociations.clear();
size_t maxCount = 20000;
for(size_t i = 0; i < vertexIds.size() && i< maxCount; ++i) {
OptimizableGraph::Vertex* _v = graph->vertex(vertexIds[i]);
g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(_v);
if (! v)
continue;
OptimizableGraph::Data* d = v->userData();
while(d) {
RGBDData* rgbdData = dynamic_cast<RGBDData*>(d);
if (!rgbdData){
d=d->next();
continue;
}
// retrieve from the rgb data the index of the parameter
int paramIndex = rgbdData->paramIndex();
// retrieve from the graph the parameter given the index
g2o::Parameter* _cameraParam = graph->parameter(paramIndex);
// attempt a cast to a parameter camera
ParameterCamera* cameraParam = dynamic_cast<ParameterCamera*>(_cameraParam);
if (! cameraParam){
cerr << "shall thou be damned forever" << endl;
return;
}
// yayyy we got the parameter
Eigen::Matrix3f cameraMatrix;
Eigen::Isometry3f sensorOffset;
cameraMatrix.setZero();
int cmax = 4;
int rmax = 3;
for (int c=0; c<cmax; c++){
for (int r=0; r<rmax; r++){
sensorOffset.matrix()(r,c)= cameraParam->offset()(r, c);
if (c<3)
cameraMatrix(r,c) = cameraParam->Kcam()(r, c);
}
}
char buf[1024];
sprintf(buf,"%d",v->id());
QString listItem(buf);
listAssociations.push_back(rgbdData);
listWidget->addItem(listItem);
d=d->next();
}
}
}
int main (int argc, char** argv) {
// Handle input
float pointSize;
float pointStep;
float alpha;
int applyTransform;
int step;
string logFilename;
string configFilename;
float di_scaleFactor;
float scale;
g2o::CommandArgs arg;
arg.param("vz_pointSize", pointSize, 1.0f, "Size of the points where are visualized");
arg.param("vz_transform", applyTransform, 1, "Choose if you want to apply the absolute transform of the point clouds");
arg.param("vz_step", step, 1, "Visualize a point cloud each vz_step point clouds");
arg.param("vz_alpha", alpha, 1.0f, "Alpha channel value used for the color points");
arg.param("vz_pointStep", pointStep, 1, "Step at which point are drawn");
arg.param("vz_scale", scale, 2, "Depth image size reduction factor");
arg.param("di_scaleFactor", di_scaleFactor, 0.001f, "Scale factor to apply to convert depth images in meters");
arg.paramLeftOver("configFilename", configFilename, "", "Configuration filename", true);
arg.paramLeftOver("logFilename", logFilename, "", "Log filename", true);
arg.parseArgs(argc, argv);
// Create GUI
QApplication application(argc,argv);
QWidget *mainWindow = new QWidget();
mainWindow->setWindowTitle("pwn_tracker_log_viewer");
QHBoxLayout *hlayout = new QHBoxLayout();
mainWindow->setLayout(hlayout);
QVBoxLayout *vlayout = new QVBoxLayout();
hlayout->addItem(vlayout);
QVBoxLayout *vlayout2 = new QVBoxLayout();
hlayout->addItem(vlayout2);
hlayout->setStretch(1, 1);
QListWidget* listWidget = new QListWidget(mainWindow);
listWidget->setSelectionMode(QAbstractItemView::MultiSelection);
vlayout->addWidget(listWidget);
PWNQGLViewer* viewer = new PWNQGLViewer(mainWindow);
vlayout2->addWidget(viewer);
Eigen::Isometry3f T;
T.setIdentity();
T.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
// Read config file
cout << "Loading config file..." << endl;
Aligner *aligner;
DepthImageConverter *converter;
std::vector<Serializable*> instances = readConfig(aligner, converter, configFilename.c_str());
converter->projector()->scale(1.0f/scale);
cout << "... done" << endl;
// Read and parse log file
std::vector<boss::Serializable*> objects;
std::vector<PwnTrackerFrame*> trackerFrames;
std::vector<PwnTrackerRelation*> trackerRelations;
Deserializer des;
des.setFilePath(logFilename);
cout << "Loading log file..." << endl;
load(trackerFrames, trackerRelations, objects, des, step);
cout << "... done" << endl;
// Load the drawable list with the PwnTrackerFrame objects
std::vector<Frame*> pointClouds;
pointClouds.resize(trackerFrames.size());
Frame *dummyFrame = 0;
std::fill(pointClouds.begin(), pointClouds.end(), dummyFrame);
for(size_t i = 0; i < trackerFrames.size(); i++) {
PwnTrackerFrame *pwnTrackerFrame = trackerFrames[i];
char nummero[1024];
sprintf(nummero, "%05d", (int)i);
listWidget->addItem(QString(nummero));
QListWidgetItem *lastItem = listWidget->item(listWidget->count() - 1);
Isometry3f transform = Isometry3f::Identity();
if(applyTransform) {
isometry3d2f(transform, pwnTrackerFrame->transform());
transform = transform*pwnTrackerFrame->sensorOffset;
}
transform.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
GLParameterFrame *frameParams = new GLParameterFrame();
frameParams->setStep(pointStep);
frameParams->setShow(false);
DrawableFrame* drawableFrame = new DrawableFrame(transform, frameParams, pointClouds[i]);
viewer->addDrawable(drawableFrame);
}
// Manage GUI
viewer->init();
mainWindow->show();
viewer->show();
listWidget->show();
viewer->setAxisIsDrawn(true);
bool selectionChanged = false;
QListWidgetItem *item = 0;
DepthImage depthImage;
DepthImage scaledDepthImage;
while (mainWindow->isVisible()) {
selectionChanged = false;
for (int i = 0; i<listWidget->count(); i++) {
item = 0;
item = listWidget->item(i);
DrawableFrame *drawableFrame = dynamic_cast<DrawableFrame*>(viewer->drawableList()[i]);
if (item && item->isSelected()) {
if(!drawableFrame->parameter()->show()) {
drawableFrame->parameter()->setShow(true);
selectionChanged = true;
}
if(pointClouds[i] == 0) {
pointClouds[i] = new Frame();
boss_map::ImageBLOB* fromDepthBlob = trackerFrames[i]->depthImage.get();
DepthImage depthImage;
depthImage.fromCvMat(fromDepthBlob->cvImage());
DepthImage::scale(scaledDepthImage, depthImage, scale);
converter->compute(*pointClouds[i], scaledDepthImage);
drawableFrame->setFrame(pointClouds[i]);
delete fromDepthBlob;
}
} else {
drawableFrame->parameter()->setShow(false);
selectionChanged = true;
}
}
if (selectionChanged)
viewer->updateGL();
application.processEvents();
}
return 0;
}
void Merger::merge(Cloud *cloud, Eigen::Isometry3f transform) {
assert(_indexImage.rows > 0 && _indexImage.cols > 0 && "Merger: _indexImage has zero size");
assert(_depthImageConverter && "Merger: missing _depthImageConverter");
assert(_depthImageConverter->projector() && "Merger: missing projector in _depthImageConverter");
PointProjector *pointProjector = _depthImageConverter->projector();
Eigen::Isometry3f oldTransform = pointProjector->transform();
pointProjector->setTransform(transform);
pointProjector->project(_indexImage,
_depthImage,
cloud->points());
int target = 0;
int distance = 0;
_collapsedIndices.resize(cloud->points().size());
std::fill(_collapsedIndices.begin(), _collapsedIndices.end(), -1);
int killed = 0;
int currentIndex = 0;
for(size_t i = 0; i < cloud->points().size(); currentIndex++ ,i++) {
const Point currentPoint = cloud->points()[i];
// const Normal currentNormal = cloud->normals()[i];
int r = -1, c = -1;
float depth = 0.0f;
pointProjector->project(c, r, depth, currentPoint);
if(depth < 0 || depth > _maxPointDepth ||
r < 0 || r >= _depthImage.rows ||
c < 0 || c >= _depthImage.cols) {
distance++;
continue;
}
float &targetZ = _depthImage(r, c);
int targetIndex = _indexImage(r, c);
if(targetIndex < 0) {
target++;
continue;
}
// const Normal &targetNormal = cloud->normals().at(targetIndex);
Eigen::Vector4f viewPointDirection = transform.matrix().col(3)-currentPoint;
viewPointDirection(3)=0;
if(targetIndex == currentIndex) {
_collapsedIndices[currentIndex] = currentIndex;
}
else if(fabs(depth - targetZ) < _distanceThreshold /*&&
currentNormal.dot(targetNormal) > _normalThreshold &&
(viewPointDirection.dot(targetNormal)>cos(0))*/ ) {
Gaussian3f &targetGaussian = cloud->gaussians()[targetIndex];
Gaussian3f ¤tGaussian = cloud->gaussians()[currentIndex];
targetGaussian.addInformation(currentGaussian);
_collapsedIndices[currentIndex] = targetIndex;
killed++;
}
}
int murdered = 0;
int k = 0;
for(size_t i = 0; i < _collapsedIndices.size(); i++) {
int collapsedIndex = _collapsedIndices[i];
if(collapsedIndex == (int)i) {
cloud->points()[i].head<3>() = cloud->gaussians()[i].mean();
}
if(collapsedIndex < 0 || collapsedIndex == (int)i) {
cloud->points()[k] = cloud->points()[i];
cloud->normals()[k] = cloud->normals()[i];
cloud->stats()[k] = cloud->stats()[i];
cloud->pointInformationMatrix()[k] = cloud->pointInformationMatrix()[i];
cloud->normalInformationMatrix()[k] = cloud->normalInformationMatrix()[i];
cloud->gaussians()[k] = cloud->gaussians()[i];
if(cloud->rgbs().size())
cloud->rgbs()[k]=cloud->rgbs()[i];
k++;
}
else {
murdered ++;
}
}
int originalSize = cloud->points().size();
// Kill the leftover points
cloud->points().resize(k);
cloud->normals().resize(k);
cloud->stats().resize(k);
cloud->pointInformationMatrix().resize(k);
cloud->normalInformationMatrix().resize(k);
if(cloud->rgbs().size())
cloud->rgbs().resize(k);
std::cout << "[INFO]: number of suppressed points " << murdered << std::endl;
std::cout << "[INFO]: resized cloud from " << originalSize << " to " << k << " points" <<std::endl;
pointProjector->setTransform(oldTransform);
}
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;
}
}
void TwoDepthImageAlignerNode::processNode(MapNode* node_){
cerr << "START ITERATION" << endl;
std::vector<Serializable*> crearedObjects;
SensingFrameNode* sensingFrame = dynamic_cast<SensingFrameNode*>(node_);
if (! sensingFrame)
return;
PinholeImageData* image = dynamic_cast<PinholeImageData*>(sensingFrame->sensorData(_topic));
if (! image)
return;
cerr << "got image" << endl;
Eigen::Isometry3d _sensorOffset = _config->sensorOffset(image->baseSensor());
// cerr << "sensorOffset: " << endl;
// cerr << _sensorOffset.matrix() << endl;
Eigen::Isometry3f sensorOffset;
convertScalar(sensorOffset,_sensorOffset);
sensorOffset.matrix().row(3) << 0,0,0,1;
Eigen::Matrix3d _cameraMatrix = image->cameraMatrix();
ImageBLOB* blob = image->imageBlob().get();
DepthImage depthImage;
depthImage.fromCvMat(blob->cvImage());
int r=depthImage.rows();
int c=depthImage.cols();
DepthImage scaledImage;
Eigen::Matrix3f cameraMatrix;
convertScalar(cameraMatrix,_cameraMatrix);
//computeScaledParameters(r,c,cameraMatrix,_scale);
PinholePointProjector* projector=dynamic_cast<PinholePointProjector*>(_converter->projector());
cameraMatrix(2,2)=1;
projector->setCameraMatrix(cameraMatrix);
projector->setImageSize(depthImage.rows(), depthImage.cols());
projector->scale(1.0/_scale);
DepthImage::scale(scaledImage,depthImage,_scale);
pwn::Frame* frame = new pwn::Frame;
_converter->compute(*frame,scaledImage, sensorOffset);
MapNodeBinaryRelation* odom=0;
std::vector<MapNode*> oneNode(1);
oneNode[0]=sensingFrame;
MapNodeUnaryRelation* imu = extractRelation<MapNodeUnaryRelation>(oneNode);
if (_previousFrame){
_aligner->setReferenceSensorOffset(_aligner->currentSensorOffset());
_aligner->setCurrentSensorOffset(sensorOffset);
_aligner->setReferenceFrame(_previousFrame);
_aligner->setCurrentFrame(frame);
PinholePointProjector* projector=(PinholePointProjector*)(_aligner->projector());
projector->setCameraMatrix(cameraMatrix);
projector->setImageSize(depthImage.rows(), depthImage.cols());
projector->scale(1.0/_scale);
_aligner->correspondenceFinder()->setImageSize(projector->imageRows(), projector->imageCols());
/*
cerr << "correspondenceFinder: " << r << " " << c << endl;
cerr << "sensorOffset" << endl;
cerr <<_aligner->currentSensorOffset().matrix() << endl;
cerr <<_aligner->referenceSensorOffset().matrix() << endl;
cerr << "cameraMatrix" << endl;
cerr << projector->cameraMatrix() << endl;
*/
std::vector<MapNode*> twoNodes(2);
twoNodes[0]=_previousSensingFrameNode;
twoNodes[1]=sensingFrame;
odom = extractRelation<MapNodeBinaryRelation>(twoNodes);
cerr << "odom:" << odom << " imu:" << imu << endl;
Eigen::Isometry3f guess= Eigen::Isometry3f::Identity();
_aligner->clearPriors();
if (odom){
Eigen::Isometry3f mean;
Eigen::Matrix<float,6,6> info;
convertScalar(mean,odom->transform());
mean.matrix().row(3) << 0,0,0,1;
convertScalar(info,odom->informationMatrix());
//cerr << "odom: " << t2v(mean).transpose() << endl;
_aligner->addRelativePrior(mean,info);
//guess = mean;
}
if (imu){
Eigen::Isometry3f mean;
Eigen::Matrix<float,6,6> info;
convertScalar(mean,imu->transform());
convertScalar(info,imu->informationMatrix());
mean.matrix().row(3) << 0,0,0,1;
//cerr << "imu: " << t2v(mean).transpose() << endl;
_aligner->addAbsolutePrior(_globalT,mean,info);
}
_aligner->setInitialGuess(guess);
cerr << "Frames: " << _previousFrame << " " << frame << endl;
// projector->setCameraMatrix(cameraMatrix);
// projector->setTransform(Eigen::Isometry3f::Identity());
// Eigen::MatrixXi debugIndices(r,c);
// DepthImage debugImage(r,c);
// projector->project(debugIndices, debugImage, frame->points());
_aligner->align();
// sprintf(buf, "img-dbg-%05d.pgm",j);
// debugImage.save(buf);
//sprintf(buf, "img-ref-%05d.pgm",j);
//_aligner->correspondenceFinder()->referenceDepthImage().save(buf);
//sprintf(buf, "img-cur-%05d.pgm",j);
//_aligner->correspondenceFinder()->currentDepthImage().save(buf);
cerr << "inliers: " << _aligner->inliers() << endl;
cerr << "chi2: " << _aligner->error() << endl;
cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
cerr << "initialGuess: " << t2v(guess).transpose() << endl;
cerr << "transform : " << t2v(_aligner->T()).transpose() << endl;
if (_aligner->inliers()>-1){
_globalT = _globalT*_aligner->T();
cerr << "TRANSFORM FOUND" << endl;
} else {
cerr << "FAILURE" << endl;
_globalT = _globalT*guess;
}
if (! (_counter%50) ) {
Eigen::Matrix3f R = _globalT.linear();
Eigen::Matrix3f E = R.transpose() * R;
E.diagonal().array() -= 1;
_globalT.linear() -= 0.5 * R * E;
}
_globalT.matrix().row(3) << 0,0,0,1;
cerr << "globalTransform : " << t2v(_globalT).transpose() << endl;
// char buf[1024];
// sprintf(buf, "frame-%05d.pwn",_counter);
// frame->save(buf, 1, true, _globalT);
cerr << "creating alias" << endl;
// create an alias for the previous node
MapNodeAlias* newRoot = new MapNodeAlias(_previousSensingFrameNode,_previousSensingFrameNode->manager());
_previousSensingFrameNode->manager()->addNode(newRoot);
cerr << "creating alias relation" << endl;
MapNodeAliasRelation* aliasRel = new MapNodeAliasRelation(newRoot,_previousSensingFrameNode->manager());
aliasRel->nodes()[0] = newRoot;
aliasRel->nodes()[1] = _previousSensingFrameNode;
_previousSensingFrameNode->manager()->addRelation(aliasRel);
cerr << "reparenting old elements" << endl;
// assign all the used relations to the alias node
if (imu){
imu->setOwner(newRoot);
imu->manager()->addRelation(imu);
}
if (odom){
odom->setOwner(newRoot);
odom->manager()->addRelation(imu);
}
cerr << "adding result" << endl;
Relation* newRel = new Relation(_aligner, _converter,
_previousSensingFrameNode, sensingFrame,
_topic, _aligner->inliers(), _aligner->error(), _manager);
newRel->setOwner(newRoot);
newRel->nodes()[0] = newRoot;
newRel->nodes()[1] = _previousSensingFrameNode;
Eigen::Isometry3d iso;
convertScalar(iso,_aligner->T());
newRel->setTransform(iso);
newRel->setInformationMatrix(Eigen::Matrix<double, 6,6>::Identity());
newRel->manager()->addRelation(newRel);
*os << _globalT.translation().transpose() << endl;
// create a new alias node for the prior element
// bind the alias with the prior node
// add to the alias the relations that have been used to
// determine the transformation
// add the alias to the map
// add the new transformation to the map
} else {
_aligner->setCurrentSensorOffset(sensorOffset);
_globalT = Eigen::Isometry3f::Identity();
if (imu){
Eigen::Isometry3f mean;
convertScalar(mean,imu->transform());
_globalT = mean;
}
}
cerr << "deleting previous frame" << endl;
if (_previousFrame)
delete _previousFrame;
cerr << "deleting blob frame" << endl;
delete blob;
cerr << "bookkeeping update" << endl;
_previousSensingFrameNode = sensingFrame;
_previousFrame = frame;
_counter++;
cerr << "END ITERATION" << endl;
}