Eigen::Matrix3f Homography::calcHomography( const Eigen::Isometry3f &camA2plane, const Eigen::Isometry3f &camB2plane ) const { // calculate the plane normal (z-vector) from the point of camB Eigen::Vector3f n = camB2plane.rotation().transpose() * -Eigen::Vector3f::UnitZ(); // calculate the distance from camB to plane float d = camB2plane.inverse().translation().dot( n ); // get transform from camB to camA Eigen::Isometry3f camBtoCamA = camA2plane.inverse() * camB2plane; // get the homography return calcHomography( camBtoCamA, n, d ); }
void KinectDatasetReader::MoveGroundTruthsToOrigin() { Eigen::Isometry3f origin = mGroundTruth[0].LocalTransform; origin = origin.inverse(); for(size_t i = 0; i < mGroundTruth.size(); i++) mGroundTruth[i].LocalTransform = origin * mGroundTruth[i].LocalTransform; }
bool Utils:: composeViewMatrix(Eigen::Projective3f& oMatrix, const Eigen::Matrix3f& iCalib, const Eigen::Isometry3f& iPose, const bool iIsOrthographic) { Eigen::Projective3f calib = Eigen::Projective3f::Identity(); calib.matrix().col(2).swap(calib.matrix().col(3)); calib.matrix().topLeftCorner<2,3>() = iCalib.matrix().topRows<2>(); calib.matrix().bottomLeftCorner<1,3>() = iCalib.matrix().bottomRows<1>(); if (iIsOrthographic) calib.matrix().col(2).swap(calib.matrix().col(3)); oMatrix = calib*iPose.inverse(); return true; }
int main(int argc, char** argv){ string dirname; string graphFilename; float numThreads; int ng_step; int ng_minPoints; int ng_imageRadius; float ng_worldRadius; float ng_maxCurvature; float al_inlierDistance; float al_inlierCurvatureRatio; float al_inlierNormalAngle; float al_inlierMaxChi2; float al_scale; float al_flatCurvatureThreshold; float al_outerIterations; float al_nonLinearIterations; float al_linearIterations; float al_minInliers; float al_lambda; float al_debug; PointWithNormalStatistcsGenerator normalGenerator; PointWithNormalAligner aligner; g2o::CommandArgs arg; arg.param("ng_step",ng_step,normalGenerator.step(),"compute a normal each x pixels") ; arg.param("ng_minPoints",ng_minPoints,normalGenerator.minPoints(),"minimum number of points in a region to compute the normal"); arg.param("ng_imageRadius",ng_imageRadius,normalGenerator.imageRadius(), "radius of a ball in the works where to compute the normal for a pixel"); arg.param("ng_worldRadius",ng_worldRadius, normalGenerator.worldRadius(), "radius of a ball in the works where to compute the normal for a pixel"); arg.param("ng_maxCurvature",ng_maxCurvature, normalGenerator.maxCurvature(), "above this threshold the normal is not computed"); arg.param("al_inlierDistance", al_inlierDistance, aligner.inlierDistanceThreshold(), "max metric distance between two points to regard them as iniliers"); arg.param("al_inlierCurvatureRatio", al_inlierCurvatureRatio, aligner.inlierCurvatureRatioThreshold(), "max metric distance between two points to regard them as iniliers"); arg.param("al_inlierNormalAngle", al_inlierNormalAngle, aligner.inlierNormalAngularThreshold(), "max metric distance between two points to regard them as iniliers"); arg.param("al_inlierMaxChi2", al_inlierMaxChi2, aligner.inlierMaxChi2(), "max metric distance between two points to regard them as iniliers"); arg.param("al_minInliers", al_minInliers, aligner.minInliers(), "minimum numver of inliers to do the matching"); arg.param("al_scale", al_scale, aligner.scale(), "scale of the range image for the alignment"); arg.param("al_flatCurvatureThreshold", al_flatCurvatureThreshold, aligner.flatCurvatureThreshold(), "curvature above which the patches are not considered planar"); arg.param("al_outerIterations", al_outerIterations, aligner.outerIterations(), "outer interations (incl. data association)"); arg.param("al_linearIterations", al_linearIterations, aligner.linearIterations(), "linear iterations for each outer one (uses R,t)"); arg.param("al_nonLinearIterations", al_nonLinearIterations, aligner.nonLinearIterations(), "nonlinear iterations for each outer one (uses q,t)"); arg.param("al_lambda", al_lambda, aligner.lambda(), "damping factor for the transformation update, the higher the smaller the step"); arg.param("al_debug", al_debug, aligner.debug(), "prints lots of stuff"); arg.param("numThreads", numThreads, 1, "numver of threads for openmp"); if (numThreads<1) numThreads = 1; std::vector<string> fileNames; arg.paramLeftOver("dirname", dirname, "", "", true); arg.paramLeftOver("graph-file", graphFilename, "out.g2o", "graph-file", true); arg.parseArgs(argc, argv); cerr << "dirname " << dirname << endl; std::vector<string> filenames; std::set<string> filenamesset = readDir(dirname); for(set<string>::const_iterator it =filenamesset.begin(); it!=filenamesset.end(); it++) { filenames.push_back(*it); } normalGenerator.setStep(ng_step); normalGenerator.setMinPoints(ng_minPoints); normalGenerator.setImageRadius(ng_imageRadius); normalGenerator.setWorldRadius(ng_worldRadius); normalGenerator.setMaxCurvature(ng_maxCurvature); #ifdef _PWN_USE_OPENMP_ normalGenerator.setNumThreads(numThreads); #endif //_PWN_USE_OPENMP_ aligner.setInlierDistanceThreshold(al_inlierDistance); aligner.setInlierCurvatureRatioThreshold(al_inlierCurvatureRatio); aligner.setInlierNormalAngularThreshold(al_inlierNormalAngle); aligner.setInlierMaxChi2(al_inlierMaxChi2); aligner.setMinInliers(al_minInliers); aligner.setScale(al_scale); aligner.setFlatCurvatureThreshold(al_flatCurvatureThreshold); aligner.setOuterIterations(al_outerIterations); aligner.setLinearIterations(al_linearIterations); aligner.setNonLinearIterations(al_nonLinearIterations); aligner.setLambda(al_lambda); aligner.setDebug(al_debug); #ifdef _PWN_USE_OPENMP_ aligner.setNumThreads(numThreads); #endif //_PWN_USE_OPENMP_ Frame* referenceFrame= 0; Eigen::Matrix3f cameraMatrix; cameraMatrix << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f; ostringstream os(graphFilename.c_str()); cerr << "there are " << filenames.size() << " files in the pool" << endl; Isometry3f trajectory; trajectory.setIdentity(); int previousIndex=-1; int graphNum=0; int nFrames = 0; string baseFilename = graphFilename.substr( 0, graphFilename.find_last_of( '.' ) ); for (size_t i=0; i<filenames.size(); i++){ cerr << endl << endl << endl; cerr << ">>>>>>>>>>>>>>>>>>>>>>>> PROCESSING " << filenames[i] << " <<<<<<<<<<<<<<<<<<<<" << endl; Frame* currentFrame= new Frame(); if(!currentFrame->load(filenames[i])) { cerr << "failure in loading image: " << filenames[i] << ", skipping" << endl; delete currentFrame; continue; } nFrames ++; if (! referenceFrame ){ os << "PARAMS_CAMERACALIB 0 0 0 0 0 0 0 1 525 525 319.5 239.5"<< endl; trajectory.setIdentity(); } { // write the vertex frame Vector6f x = t2v(trajectory); Vector3f t = x.head<3>(); Vector3f mq = x.tail<3>(); float w = mq.squaredNorm(); if (w>1){ mq.setZero(); w = 1.0f; } else { w = sqrt(1-w); } os << "VERTEX_SE3:QUAT " << i << " " << t.transpose() << " " << mq.transpose() << " " << w << endl; os << "DEPTH_IMAGE_DATA 0 " << filenames[i] << " 0 0 " << endl; } currentFrame->computeStats(normalGenerator,cameraMatrix); if (referenceFrame) { referenceFrame->setAligner(aligner, true); currentFrame->setAligner(aligner, false); Matrix6f omega; Vector6f mean; float tratio; float rratio; aligner.setImageSize(currentFrame->depthImage.rows(), currentFrame->depthImage.cols()); Eigen::Isometry3f X; X.setIdentity(); double ostart = get_time(); float error; int result = aligner.align(error, X, mean, omega, tratio, rratio); cerr << "inliers=" << result << " error/inliers: " << error/result << endl; cerr << "localTransform : " << endl; cerr << X.inverse().matrix() << endl; trajectory=trajectory*X; cerr << "globaltransform: " << endl; cerr << trajectory.matrix() << endl; double oend = get_time(); cerr << "alignment took: " << oend-ostart << " sec." << endl; cerr << "aligner scaled image size: " << aligner.scaledImageRows() << " " << aligner.scaledImageCols() << endl; if(rratio < 100 && tratio < 100) { // write the edge frame Vector6f x = mean; Vector3f t = x.head<3>(); Vector3f mq = x.tail<3>(); float w = mq.squaredNorm(); if (w>1){ mq.setZero(); w = 1.0f; } else { w = sqrt(1-w); } os << "EDGE_SE3:QUAT " << previousIndex << " " << i << " "; os << t.transpose() << " " << mq.transpose() << " " << w << " "; for (int r=0; r<6; r++){ for (int c=r; c<6; c++){ os << omega(r,c) << " "; } } os << endl; } else { if (nFrames >10) { char buf[1024]; sprintf(buf, "%s-%03d.g2o", baseFilename.c_str(), graphNum); ofstream gs(buf); gs << os.str(); gs.close(); } os.str(""); os.clear(); if (referenceFrame) delete referenceFrame; referenceFrame = 0; graphNum ++; nFrames = 0; } } previousIndex = i; if (referenceFrame) delete referenceFrame; referenceFrame = currentFrame; } char buf[1024]; sprintf(buf, "%s-%03d.g2o", baseFilename.c_str(), graphNum); cerr << "saving final frames, n: " << nFrames << " in file [" << buf << "]" << endl; cerr << "filesize:" << os.str().length() << endl; ofstream gs(buf); gs << os.str(); cout << os.str(); gs.close(); }
void ICPOdometry::getIncrementalTransformation(Eigen::Vector3f & trans, Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & rot, int threads, int blocks) { iterations[0] = 10; iterations[1] = 5; iterations[2] = 4; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev = rot; Eigen::Vector3f tprev = trans; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr = Rprev; Eigen::Vector3f tcurr = tprev; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev_inv = Rprev.inverse(); Mat33 & device_Rprev_inv = device_cast<Mat33>(Rprev_inv); float3& device_tprev = device_cast<float3>(tprev); cv::Mat resultRt = cv::Mat::eye(4, 4, CV_64FC1); for(int i = NUM_PYRS - 1; i >= 0; i--) { for(int j = 0; j < iterations[i]; j++) { Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_icp; Eigen::Matrix<float, 6, 1> b_icp; Mat33& device_Rcurr = device_cast<Mat33> (Rcurr); float3& device_tcurr = device_cast<float3>(tcurr); DeviceArray2D<float>& vmap_curr = vmaps_curr_[i]; DeviceArray2D<float>& nmap_curr = nmaps_curr_[i]; DeviceArray2D<float>& vmap_g_prev = vmaps_g_prev_[i]; DeviceArray2D<float>& nmap_g_prev = nmaps_g_prev_[i]; float residual[2]; icpStep(device_Rcurr, device_tcurr, vmap_curr, nmap_curr, device_Rprev_inv, device_tprev, intr(i), vmap_g_prev, nmap_g_prev, distThres_, angleThres_, sumData, outData, A_icp.data(), b_icp.data(), &residual[0], threads, blocks); lastICPError = sqrt(residual[0]) / residual[1]; lastICPCount = residual[1]; Eigen::Matrix<double, 6, 1> result; Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_icp = A_icp.cast<double>(); Eigen::Matrix<double, 6, 1> db_icp = b_icp.cast<double>(); lastA = dA_icp; lastb = db_icp; result = lastA.ldlt().solve(lastb); Eigen::Isometry3f incOdom; OdometryProvider::computeProjectiveMatrix(resultRt, result, incOdom); Eigen::Isometry3f currentT; currentT.setIdentity(); currentT.rotate(Rprev); currentT.translation() = tprev; currentT = currentT * incOdom.inverse(); tcurr = currentT.translation(); Rcurr = currentT.rotation(); } } trans = tcurr; rot = Rcurr; }
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 PointWithNormalStatistcsGenerator::computeNormalsAndSVD(PointWithNormalVector& points, PointWithNormalSVDVector& svds, const Eigen::MatrixXi& indices, const Eigen::Matrix3f& cameraMatrix, const Eigen::Isometry3f& transform){ _integralImage.compute(indices,points); int q=0; int outerStep = _numThreads * _step; PixelMapper pixelMapper; pixelMapper.setCameraMatrix(cameraMatrix); pixelMapper.setTransform(transform); Eigen::Isometry3f inverseTransform = transform.inverse(); #pragma omp parallel { #ifdef _PWN_USE_OPENMP_ int threadNum = omp_get_thread_num(); #else // _PWN_USE_OPENMP_ int threadNum = 0; Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver; #endif // _PWN_USE_OPENMP_ for (int c=threadNum; c<indices.cols(); c+=outerStep) { #ifdef _PWN_USE_OPENMP_ Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver; #endif // _PWN_USE_OPENMP_ for (int r=0; r<indices.rows(); r+=_step, q++){ int index = indices(r,c); //cerr << "index(" << r <<"," << c << ")=" << index << endl; if (index<0) continue; // determine the region PointWithNormal& point = points[index]; PointWithNormalSVD& originalSVD = svds[index]; PointWithNormalSVD svd; Eigen::Vector3f normal = point.normal(); Eigen::Vector3f coord = pixelMapper.projectPoint(point.point()+Eigen::Vector3f(_worldRadius, _worldRadius, 0)); svd._z=point(2); coord.head<2>()*=(1./coord(2)); int dx = abs(c - coord[0]); int dy = abs(r - coord[1]); if (dx>_imageRadius) dx = _imageRadius; if (dy>_imageRadius) dy = _imageRadius; PointAccumulator acc = _integralImage.getRegion(c-dx, c+dx, r-dy, r+dy); svd._mean=point.point(); if (acc.n()>_minPoints){ Eigen::Vector3f mean = acc.mean(); svd._mean = mean; svd._n = acc.n(); Eigen::Matrix3f cov = acc.covariance(); eigenSolver.compute(cov); svd._U=eigenSolver.eigenvectors(); svd._singularValues=eigenSolver.eigenvalues(); if (svd._singularValues(0) <0) svd._singularValues(0) = 0; /* cerr << "\t svd.singularValues():" << svd.singularValues() << endl; cerr << "\t svd.U():" << endl << svd.U() << endl; //cerr << "\t svd.curvature():" << svd.curvature() << endl; */ normal = eigenSolver.eigenvectors().col(0).normalized(); if (normal.dot(inverseTransform * mean) > 0.0f) normal =-normal; svd.updateCurvature(); //cerr << "n(" << index << ") c:" << svd.curvature() << endl << point.tail<3>() << endl; if (svd.curvature()>_maxCurvature){ //cerr << "region: " << c-dx << " " << c+dx << " " << r-dx << " " << r+dx << " points: " << acc.n() << endl; normal.setZero(); } } else { normal.setZero(); svd = PointWithNormalSVD(); } if (svd.n() > originalSVD.n()){ originalSVD = svd; point.setNormal(normal); } } } } }
void RGBDOdometry::getIncrementalTransformation(Eigen::Vector3f & trans, Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & rot, const bool & rgbOnly, const float & icpWeight, const bool & pyramid, const bool & fastOdom, const bool & so3) { bool icp = !rgbOnly && icpWeight > 0; bool rgb = rgbOnly || icpWeight < 100; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev = rot; Eigen::Vector3f tprev = trans; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr = Rprev; Eigen::Vector3f tcurr = tprev; if(rgb) { for(int i = 0; i < NUM_PYRS; i++) { computeDerivativeImages(nextImage[i], nextdIdx[i], nextdIdy[i]); } } Eigen::Matrix<double, 3, 3, Eigen::RowMajor> resultR = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Identity(); if(so3) { int pyramidLevel = 2; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> R_lr = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Identity(); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero(); K(0, 0) = intr(pyramidLevel).fx; K(1, 1) = intr(pyramidLevel).fy; K(0, 2) = intr(pyramidLevel).cx; K(1, 2) = intr(pyramidLevel).cy; K(2, 2) = 1; float lastError = std::numeric_limits<float>::max() / 2; float lastCount = std::numeric_limits<float>::max() / 2; Eigen::Matrix<double, 3, 3, Eigen::RowMajor> lastResultR = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Identity(); for(int i = 0; i < 10; i++) { Eigen::Matrix<float, 3, 3, Eigen::RowMajor> jtj; Eigen::Matrix<float, 3, 1> jtr; Eigen::Matrix<double, 3, 3, Eigen::RowMajor> homography = K * resultR * K.inverse(); mat33 imageBasis; memcpy(&imageBasis.data[0], homography.cast<float>().eval().data(), sizeof(mat33)); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_inv = K.inverse(); mat33 kinv; memcpy(&kinv.data[0], K_inv.cast<float>().eval().data(), sizeof(mat33)); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_R_lr = K * resultR; mat33 krlr; memcpy(&krlr.data[0], K_R_lr.cast<float>().eval().data(), sizeof(mat33)); float residual[2]; TICK("so3Step"); so3Step(lastNextImage[pyramidLevel], nextImage[pyramidLevel], imageBasis, kinv, krlr, sumDataSO3, outDataSO3, jtj.data(), jtr.data(), &residual[0], GPUConfig::getInstance().so3StepThreads, GPUConfig::getInstance().so3StepBlocks); TOCK("so3Step"); lastSO3Error = sqrt(residual[0]) / residual[1]; lastSO3Count = residual[1]; //Converged if(lastSO3Error < lastError && lastCount == lastSO3Count) { break; } else if(lastSO3Error > lastError + 0.001) //Diverging { lastSO3Error = lastError; lastSO3Count = lastCount; resultR = lastResultR; break; } lastError = lastSO3Error; lastCount = lastSO3Count; lastResultR = resultR; Eigen::Vector3f delta = jtj.ldlt().solve(jtr); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rotUpdate = OdometryProvider::rodrigues(delta.cast<double>()); R_lr = rotUpdate.cast<float>() * R_lr; for(int x = 0; x < 3; x++) { for(int y = 0; y < 3; y++) { resultR(x, y) = R_lr(x, y); } } } } iterations[0] = fastOdom ? 3 : 10; iterations[1] = pyramid ? 5 : 0; iterations[2] = pyramid ? 4 : 0; Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev_inv = Rprev.inverse(); mat33 device_Rprev_inv = Rprev_inv; float3 device_tprev = *reinterpret_cast<float3*>(tprev.data()); Eigen::Matrix<double, 4, 4, Eigen::RowMajor> resultRt = Eigen::Matrix<double, 4, 4, Eigen::RowMajor>::Identity(); if(so3) { for(int x = 0; x < 3; x++) { for(int y = 0; y < 3; y++) { resultRt(x, y) = resultR(x, y); } } } for(int i = NUM_PYRS - 1; i >= 0; i--) { if(rgb) { projectToPointCloud(lastDepth[i], pointClouds[i], intr, i); } Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero(); K(0, 0) = intr(i).fx; K(1, 1) = intr(i).fy; K(0, 2) = intr(i).cx; K(1, 2) = intr(i).cy; K(2, 2) = 1; lastRGBError = std::numeric_limits<float>::max(); for(int j = 0; j < iterations[i]; j++) { Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Rt = resultRt.inverse(); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R = Rt.topLeftCorner(3, 3); Eigen::Matrix<double, 3, 3, Eigen::RowMajor> KRK_inv = K * R * K.inverse(); mat33 krkInv; memcpy(&krkInv.data[0], KRK_inv.cast<float>().eval().data(), sizeof(mat33)); Eigen::Vector3d Kt = Rt.topRightCorner(3, 1); Kt = K * Kt; float3 kt = {(float)Kt(0), (float)Kt(1), (float)Kt(2)}; int sigma = 0; int rgbSize = 0; if(rgb) { TICK("computeRgbResidual"); computeRgbResidual(pow(minimumGradientMagnitudes[i], 2.0) / pow(sobelScale, 2.0), nextdIdx[i], nextdIdy[i], lastDepth[i], nextDepth[i], lastImage[i], nextImage[i], corresImg[i], sumResidualRGB, maxDepthDeltaRGB, kt, krkInv, sigma, rgbSize, GPUConfig::getInstance().rgbResThreads, GPUConfig::getInstance().rgbResBlocks); TOCK("computeRgbResidual"); } float sigmaVal = std::sqrt((float)sigma / rgbSize == 0 ? 1 : rgbSize); float rgbError = std::sqrt(sigma) / (rgbSize == 0 ? 1 : rgbSize); if(rgbOnly && rgbError > lastRGBError) { break; } lastRGBError = rgbError; lastRGBCount = rgbSize; if(rgbOnly) { sigmaVal = -1; //Signals the internal optimisation to weight evenly } Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_icp; Eigen::Matrix<float, 6, 1> b_icp; mat33 device_Rcurr = Rcurr; float3 device_tcurr = *reinterpret_cast<float3*>(tcurr.data()); DeviceArray2D<float>& vmap_curr = vmaps_curr_[i]; DeviceArray2D<float>& nmap_curr = nmaps_curr_[i]; DeviceArray2D<float>& vmap_g_prev = vmaps_g_prev_[i]; DeviceArray2D<float>& nmap_g_prev = nmaps_g_prev_[i]; float residual[2]; if(icp) { TICK("icpStep"); icpStep(device_Rcurr, device_tcurr, vmap_curr, nmap_curr, device_Rprev_inv, device_tprev, intr(i), vmap_g_prev, nmap_g_prev, distThres_, angleThres_, sumDataSE3, outDataSE3, A_icp.data(), b_icp.data(), &residual[0], GPUConfig::getInstance().icpStepThreads, GPUConfig::getInstance().icpStepBlocks); TOCK("icpStep"); } lastICPError = sqrt(residual[0]) / residual[1]; lastICPCount = residual[1]; Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_rgbd; Eigen::Matrix<float, 6, 1> b_rgbd; if(rgb) { TICK("rgbStep"); rgbStep(corresImg[i], sigmaVal, pointClouds[i], intr(i).fx, intr(i).fy, nextdIdx[i], nextdIdy[i], sobelScale, sumDataSE3, outDataSE3, A_rgbd.data(), b_rgbd.data(), GPUConfig::getInstance().rgbStepThreads, GPUConfig::getInstance().rgbStepBlocks); TOCK("rgbStep"); } Eigen::Matrix<double, 6, 1> result; Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_rgbd = A_rgbd.cast<double>(); Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_icp = A_icp.cast<double>(); Eigen::Matrix<double, 6, 1> db_rgbd = b_rgbd.cast<double>(); Eigen::Matrix<double, 6, 1> db_icp = b_icp.cast<double>(); if(icp && rgb) { double w = icpWeight; lastA = dA_rgbd + w * w * dA_icp; lastb = db_rgbd + w * db_icp; result = lastA.ldlt().solve(lastb); } else if(icp) { lastA = dA_icp; lastb = db_icp; result = lastA.ldlt().solve(lastb); } else if(rgb) { lastA = dA_rgbd; lastb = db_rgbd; result = lastA.ldlt().solve(lastb); } else { assert(false && "Control shouldn't reach here"); } Eigen::Isometry3f rgbOdom; OdometryProvider::computeUpdateSE3(resultRt, result, rgbOdom); Eigen::Isometry3f currentT; currentT.setIdentity(); currentT.rotate(Rprev); currentT.translation() = tprev; currentT = currentT * rgbOdom.inverse(); tcurr = currentT.translation(); Rcurr = currentT.rotation(); } } if(rgb && (tcurr - tprev).norm() > 0.3) { Rcurr = Rprev; tcurr = tprev; } if(so3) { for(int i = 0; i < NUM_PYRS; i++) { std::swap(lastNextImage[i], nextImage[i]); } } trans = tcurr; rot = Rcurr; }
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; } }