void ViewerState::processCommands(){ _meHasNewFrame = false; refreshFlags(); updateDrawableParameters(); if(!wasInitialGuess && !newCloudAdded && drawableFrameVector.size() > 1 && *initialGuessViewer) { initialGuessSelected(); continuousMode = false; } else if(newCloudAdded && drawableFrameVector.size() > 1 && *optimizeViewer && !(*stepByStepViewer)) { optimizeSelected(); continuousMode = false; } // Add cloud was pressed. else if(*addCloud) { addCloudSelected(); continuousMode = false; } // clear buttons pressed. else if(*clearAll) { clear(); *clearAll = 0; continuousMode = false; } else if(*clearLast) { clearLastSelected(); continuousMode = false; } else if(continuousMode){ addNextAndOptimizeSelected(); } // To avoid memorized commands to be managed. *initialGuessViewer = 0; *optimizeViewer = 0; *addCloud = 0; *clearAll = 0; *clearLast = 0; if (0 && drawableFrameVector.size()){ Eigen::Isometry3f globalT = drawableFrameVector.front()->transformation(); qglviewer::Vec robotPose(globalT.translation().x(), globalT.translation().y(), globalT.translation().z()); qglviewer::Vec robotAxisX(globalT.linear()(0,0), globalT.linear()(1,0), globalT.linear()(2,0)); qglviewer::Vec robotAxisZ(globalT.linear()(0,2), globalT.linear()(1,2), globalT.linear()(2,2)); pwnGMW->viewer_3d->camera()->setPosition(robotPose+.5*robotAxisZ+.5*robotAxisX); pwnGMW->viewer_3d->camera()->setUpVector(robotAxisX+robotAxisZ); pwnGMW->viewer_3d->camera()->setViewDirection(robotPose+.5*robotAxisZ+.5*robotAxisX); } pwnGMW->viewer_3d->updateGL(); }
void DrawableCloud::draw() { if(_parameter->show() && _cloud) { glPushMatrix(); glMultMatrixf(_transformation.data()); if(_drawablePoints) _drawablePoints->draw(); if(_drawableNormals) _drawableNormals->draw(); if(_drawableCovariances) _drawableCovariances->draw(); if(_drawableCorrespondences) _drawableCorrespondences->draw(); glPushMatrix(); Eigen::Isometry3f sensorOffset; sensorOffset.translation() = Eigen::Vector3f(0.0f, 0.0f, 0.0f); Eigen::Quaternionf quaternion = Eigen::Quaternionf(-.5f, -0.5f, 0.5f, 0.5f); sensorOffset.linear() = quaternion.toRotationMatrix(); sensorOffset.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f; glMultMatrixf(sensorOffset.data()); glColor4f(1,0,0,0.5); glPopMatrix(); glPopMatrix(); } }
void DrawableTransformCovariance::updateCovarianceDrawList() { GLParameterTransformCovariance *covarianceParameter = dynamic_cast<GLParameterTransformCovariance*>(_parameter); glNewList(_covarianceDrawList, GL_COMPILE); if(_covariance != Eigen::Matrix3f::Zero() && covarianceParameter && covarianceParameter->show() && covarianceParameter->scale() > 0.0f) { float scale = covarianceParameter->scale(); Eigen::Vector4f color = covarianceParameter->color(); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver; eigenSolver.computeDirect(_covariance, Eigen::ComputeEigenvectors); Eigen::Vector3f lambda = eigenSolver.eigenvalues(); Eigen::Isometry3f I = Eigen::Isometry3f::Identity(); I.linear() = eigenSolver.eigenvectors(); I.translation() = Eigen::Vector3f(_mean.x(), _mean.y(), _mean.z()); float sx = sqrt(lambda[0]) * scale; float sy = sqrt(lambda[1]) * scale; float sz = sqrt(lambda[2]) * scale; glPushMatrix(); glMultMatrixf(I.data()); glColor4f(color[0], color[1], color[2], color[3]); glScalef(sx, sy, sz); glCallList(_sphereDrawList); glPopMatrix(); } glEndList(); }
void DrawableUncertainty::updateCovarianceDrawList() { GLParameterUncertainty *uncertaintyParameter = dynamic_cast<GLParameterUncertainty*>(_parameter); glNewList(_covarianceDrawList, GL_COMPILE); if(_covarianceDrawList && _covariances && uncertaintyParameter && uncertaintyParameter->ellipsoidScale() > 0.0f) { uncertaintyParameter->applyGLParameter(); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver; float ellipsoidScale = uncertaintyParameter->ellipsoidScale(); for(size_t i = 0; i < _covariances->size(); i += uncertaintyParameter->step()) { Gaussian3f &gaussian3f = _covariances->at(i); Eigen::Matrix3f covariance = gaussian3f.covarianceMatrix(); Eigen::Vector3f mean = gaussian3f.mean(); eigenSolver.computeDirect(covariance, Eigen::ComputeEigenvectors); Eigen::Vector3f eigenValues = eigenSolver.eigenvalues(); Eigen::Isometry3f I = Eigen::Isometry3f::Identity(); I.linear() = eigenSolver.eigenvectors(); I.translation() = mean; float sx = sqrt(eigenValues[0]) * ellipsoidScale; float sy = sqrt(eigenValues[1]) * ellipsoidScale; float sz = sqrt(eigenValues[2]) * ellipsoidScale; glPushMatrix(); glMultMatrixf(I.data()); sx = sx; sy = sy; sz = sz; glScalef(sx, sy, sz); glCallList(_sphereDrawList); glPopMatrix(); } } glEndList(); }
TEST (PCL, GeneralizedIterativeClosestPoint) { typedef PointXYZ PointT; PointCloud<PointT>::Ptr src (new PointCloud<PointT>); copyPointCloud (cloud_source, *src); PointCloud<PointT>::Ptr tgt (new PointCloud<PointT>); copyPointCloud (cloud_target, *tgt); PointCloud<PointT> output; GeneralizedIterativeClosestPoint<PointT, PointT> reg; reg.setInputSource (src); reg.setInputTarget (tgt); reg.setMaximumIterations (50); reg.setTransformationEpsilon (1e-8); // Register reg.align (output); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); EXPECT_LT (reg.getFitnessScore (), 0.0001); // Check again, for all possible caching schemes for (int iter = 0; iter < 4; iter++) { bool force_cache = (bool) iter/2; bool force_cache_reciprocal = (bool) iter%2; pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); // Ensure that, when force_cache is not set, we are robust to the wrong input if (force_cache) tree->setInputCloud (tgt); reg.setSearchMethodTarget (tree, force_cache); pcl::search::KdTree<PointT>::Ptr tree_recip (new pcl::search::KdTree<PointT>); if (force_cache_reciprocal) tree_recip->setInputCloud (src); reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); // Register reg.align (output); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); EXPECT_LT (reg.getFitnessScore (), 0.001); } // Test guess matrix Eigen::Isometry3f transform = Eigen::Isometry3f (Eigen::AngleAxisf (0.25 * M_PI, Eigen::Vector3f::UnitX ()) * Eigen::AngleAxisf (0.50 * M_PI, Eigen::Vector3f::UnitY ()) * Eigen::AngleAxisf (0.33 * M_PI, Eigen::Vector3f::UnitZ ())); transform.translation () = Eigen::Vector3f (0.1, 0.2, 0.3); PointCloud<PointT>::Ptr transformed_tgt (new PointCloud<PointT>); pcl::transformPointCloud (*tgt, *transformed_tgt, transform.matrix ()); // transformed_tgt is now a copy of tgt with a transformation matrix applied GeneralizedIterativeClosestPoint<PointT, PointT> reg_guess; reg_guess.setInputSource (src); reg_guess.setInputTarget (transformed_tgt); reg_guess.setMaximumIterations (50); reg_guess.setTransformationEpsilon (1e-8); reg_guess.align (output, transform.matrix ()); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); EXPECT_LT (reg.getFitnessScore (), 0.0001); }
void draw(const std::shared_ptr<maps::ViewBase>& iView, const std::shared_ptr<MeshRenderer>& iMeshRenderer) { if (!mVisible) return; // set mesh properties iMeshRenderer->setRangeOrigin(mLatestTransform.translation()); iMeshRenderer->setScaleRange(mAttributes.mMinZ, mAttributes.mMaxZ); iMeshRenderer->setPointSize(mAttributes.mPointSize); Eigen::Projective3f worldToMap = mUseTransform ? iView->getTransform() : Eigen::Projective3f::Identity(); Eigen::Projective3f mapToWorld = worldToMap.inverse(); Eigen::Matrix3f calib; Eigen::Isometry3f pose; bool ortho; maps::Utils::factorViewMatrix(worldToMap, calib, pose, ortho); iMeshRenderer->setNormalZero(-pose.linear().col(2)); // see whether we need to (and can) get a mesh representation bool usePoints = false; maps::TriangleMesh::Ptr mesh; if (mAttributes.mMeshMode == MeshRenderer::MeshModePoints) { usePoints = true; } else { mesh = iView->getAsMesh(!mUseTransform); if (mesh == NULL) usePoints = true; } // just a point cloud if (usePoints) { mesh.reset(new maps::TriangleMesh()); maps::PointCloud::Ptr cloud = iView->getAsPointCloud(!mUseTransform); mesh->mVertices.reserve(cloud->size()); for (size_t i = 0; i < cloud->size(); ++i) { mesh->mVertices.push_back((*cloud)[i].getVector3fMap()); } } // set up mesh renderer iMeshRenderer->setColor(mAttributes.mColor[0], mAttributes.mColor[1], mAttributes.mColor[2]); iMeshRenderer->setColorMode ((MeshRenderer::ColorMode)mAttributes.mColorMode); iMeshRenderer->setMeshMode ((MeshRenderer::MeshMode)mAttributes.mMeshMode); if (usePoints) iMeshRenderer->setMeshMode(MeshRenderer::MeshModePoints); iMeshRenderer->setData(mesh->mVertices, mesh->mNormals, mesh->mFaces, mapToWorld); // draw this view's data iMeshRenderer->draw(); drawBounds(); }
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 DrawableCovariances::updateCovarianceDrawList() { GLParameterCovariances *covariancesParameter = dynamic_cast<GLParameterCovariances*>(_parameter); glNewList(_covarianceDrawList, GL_COMPILE); if(_covariances && covariancesParameter && covariancesParameter->show() && covariancesParameter->ellipsoidScale() > 0.0f) { float ellipsoidScale = covariancesParameter->ellipsoidScale(); Eigen::Vector4f colorLowCurvature = covariancesParameter->colorLowCurvature(); Eigen::Vector4f colorHighCurvature = covariancesParameter->colorHighCurvature(); float curvatureThreshold = covariancesParameter->curvatureThreshold(); for(size_t i = 0; i < _covariances->size(); i += covariancesParameter->step()) { Stats cov = _covariances->at(i); Eigen::Vector3f lambda = cov.eigenValues(); Eigen::Isometry3f I = Eigen::Isometry3f::Identity(); I.linear() = cov.eigenVectors(); if(cov.n() == 0 ) continue; I.translation() = Eigen::Vector3f(cov.mean()[0], cov.mean()[1], cov.mean()[2]); float sx = sqrt(lambda[0]) * ellipsoidScale; float sy = sqrt(lambda[1]) * ellipsoidScale; float sz = sqrt(lambda[2]) * ellipsoidScale; float curvature = cov.curvature(); glPushMatrix(); glMultMatrixf(I.data()); if(curvature > curvatureThreshold) { glColor4f(colorHighCurvature[0] - curvature, colorHighCurvature[1], colorHighCurvature[2], colorHighCurvature[3]); sx = ellipsoidScale; sy = ellipsoidScale; sz = ellipsoidScale; } else { glColor4f(colorLowCurvature[0], colorLowCurvature[1] - curvature, colorLowCurvature[2], colorLowCurvature[3]); sx = 1e-03; sy = ellipsoidScale; sz = ellipsoidScale; } glScalef(sx, sy, sz); glCallList(_sphereDrawList); glPopMatrix(); } } glEndList(); }
Eigen::Isometry3f LDrawPartReference::getTransform() { Eigen::Isometry3f tf = Eigen::Isometry3f::Identity(); for (int i = 0; i < 3; ++i) { tf.translation()[i] = position[i] * LDRAW_UNITS_TO_M; } Eigen::Matrix3f rot; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { rot(i,j) = rotation[i][j]; } } tf.rotate(rot); return tf; }
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 Utils:: factorViewMatrix(const Eigen::Projective3f& iMatrix, Eigen::Matrix3f& oCalib, Eigen::Isometry3f& oPose, bool& oIsOrthographic) { oIsOrthographic = isOrthographic(iMatrix.matrix()); // get appropriate rows std::vector<int> rows = {0,1,2}; if (!oIsOrthographic) rows[2] = 3; // get A matrix (upper left 3x3) and t vector Eigen::Matrix3f A; Eigen::Vector3f t; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { A(i,j) = iMatrix(rows[i],j); } t[i] = iMatrix(rows[i],3); } // determine translation vector oPose.setIdentity(); oPose.translation() = -(A.inverse()*t); // determine calibration matrix Eigen::Matrix3f AAtrans = A*A.transpose(); AAtrans.col(0).swap(AAtrans.col(2)); AAtrans.row(0).swap(AAtrans.row(2)); Eigen::LLT<Eigen::Matrix3f, Eigen::Upper> llt(AAtrans); oCalib = llt.matrixU(); oCalib.col(0).swap(oCalib.col(2)); oCalib.row(0).swap(oCalib.row(2)); oCalib.transposeInPlace(); // compute rotation matrix oPose.linear() = (oCalib.inverse()*A).transpose(); return true; }
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; }
Eigen::Matrix3f Homography::calcHomography( const Eigen::Isometry3f &trans, const Eigen::Vector3f &n, float dist ) const { return calcHomography( trans.rotation(), trans.translation(), n, dist ); }
BlockFitter::Result BlockFitter:: go() { Result result; result.mSuccess = false; if (mCloud->size() < 100) return result; // voxelize LabeledCloud::Ptr cloud(new LabeledCloud()); pcl::VoxelGrid<pcl::PointXYZL> voxelGrid; voxelGrid.setInputCloud(mCloud); voxelGrid.setLeafSize(mDownsampleResolution, mDownsampleResolution, mDownsampleResolution); voxelGrid.filter(*cloud); for (int i = 0; i < (int)cloud->size(); ++i) cloud->points[i].label = i; if (mDebug) { std::cout << "Original cloud size " << mCloud->size() << std::endl; std::cout << "Voxelized cloud size " << cloud->size() << std::endl; pcl::io::savePCDFileBinary("cloud_full.pcd", *cloud); } if (cloud->size() < 100) return result; // pose cloud->sensor_origin_.head<3>() = mOrigin; cloud->sensor_origin_[3] = 1; Eigen::Vector3f rz = mLookDir; Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ()); Eigen::Vector3f ry = rz.cross(rx); Eigen::Matrix3f rotation; rotation.col(0) = rx.normalized(); rotation.col(1) = ry.normalized(); rotation.col(2) = rz.normalized(); Eigen::Isometry3f pose = Eigen::Isometry3f::Identity(); pose.linear() = rotation; pose.translation() = mOrigin; // ground removal if (mRemoveGround) { Eigen::Vector4f groundPlane; // filter points float minZ = mMinGroundZ; float maxZ = mMaxGroundZ; if ((minZ > 10000) && (maxZ > 10000)) { std::vector<float> zVals(cloud->size()); for (int i = 0; i < (int)cloud->size(); ++i) { zVals[i] = cloud->points[i].z; } std::sort(zVals.begin(), zVals.end()); minZ = zVals[0]-0.1; maxZ = minZ + 0.5; } LabeledCloud::Ptr tempCloud(new LabeledCloud()); for (int i = 0; i < (int)cloud->size(); ++i) { const Eigen::Vector3f& p = cloud->points[i].getVector3fMap(); if ((p[2] < minZ) || (p[2] > maxZ)) continue; tempCloud->push_back(cloud->points[i]); } // downsample voxelGrid.setInputCloud(tempCloud); voxelGrid.setLeafSize(0.1, 0.1, 0.1); voxelGrid.filter(*tempCloud); if (tempCloud->size() < 100) return result; // find ground plane std::vector<Eigen::Vector3f> pts(tempCloud->size()); for (int i = 0; i < (int)tempCloud->size(); ++i) { pts[i] = tempCloud->points[i].getVector3fMap(); } const float kGroundPlaneDistanceThresh = 0.01; // TODO: param PlaneFitter planeFitter; planeFitter.setMaxDistance(kGroundPlaneDistanceThresh); planeFitter.setRefineUsingInliers(true); auto res = planeFitter.go(pts); groundPlane = res.mPlane; if (groundPlane[2] < 0) groundPlane = -groundPlane; if (mDebug) { std::cout << "dominant plane: " << groundPlane.transpose() << std::endl; std::cout << " inliers: " << res.mInliers.size() << std::endl; } if (std::acos(groundPlane[2]) > 30*M_PI/180) { std::cout << "error: ground plane not found!" << std::endl; std::cout << "proceeding with full segmentation (may take a while)..." << std::endl; } else { // compute convex hull result.mGroundPlane = groundPlane; { tempCloud.reset(new LabeledCloud()); for (int i = 0; i < (int)cloud->size(); ++i) { Eigen::Vector3f p = cloud->points[i].getVector3fMap(); float dist = groundPlane.head<3>().dot(p) + groundPlane[3]; if (std::abs(dist) > kGroundPlaneDistanceThresh) continue; p -= (groundPlane.head<3>()*dist); pcl::PointXYZL cloudPt; cloudPt.getVector3fMap() = p; tempCloud->push_back(cloudPt); } pcl::ConvexHull<pcl::PointXYZL> chull; pcl::PointCloud<pcl::PointXYZL> hull; chull.setInputCloud(tempCloud); chull.reconstruct(hull); result.mGroundPolygon.resize(hull.size()); for (int i = 0; i < (int)hull.size(); ++i) { result.mGroundPolygon[i] = hull[i].getVector3fMap(); } } // remove points below or near ground tempCloud.reset(new LabeledCloud()); for (int i = 0; i < (int)cloud->size(); ++i) { Eigen::Vector3f p = cloud->points[i].getVector3fMap(); float dist = p.dot(groundPlane.head<3>()) + groundPlane[3]; if ((dist < mMinHeightAboveGround) || (dist > mMaxHeightAboveGround)) continue; float range = (p-mOrigin).norm(); if (range > mMaxRange) continue; tempCloud->push_back(cloud->points[i]); } std::swap(tempCloud, cloud); if (mDebug) { std::cout << "Filtered cloud size " << cloud->size() << std::endl; } } } // normal estimation auto t0 = std::chrono::high_resolution_clock::now(); if (mDebug) { std::cout << "computing normals..." << std::flush; } RobustNormalEstimator normalEstimator; normalEstimator.setMaxEstimationError(0.01); normalEstimator.setRadius(0.1); normalEstimator.setMaxCenterError(0.02); normalEstimator.setMaxIterations(100); NormalCloud::Ptr normals(new NormalCloud()); normalEstimator.go(cloud, *normals); if (mDebug) { auto t1 = std::chrono::high_resolution_clock::now(); auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0); std::cout << "finished in " << dt.count()/1e3 << " sec" << std::endl; } // filter non-horizontal points const float maxNormalAngle = mMaxAngleFromHorizontal*M_PI/180; LabeledCloud::Ptr tempCloud(new LabeledCloud()); NormalCloud::Ptr tempNormals(new NormalCloud()); for (int i = 0; i < (int)normals->size(); ++i) { const auto& norm = normals->points[i]; Eigen::Vector3f normal(norm.normal_x, norm.normal_y, norm.normal_z); float angle = std::acos(normal[2]); if (angle > maxNormalAngle) continue; tempCloud->push_back(cloud->points[i]); tempNormals->push_back(normals->points[i]); } std::swap(tempCloud, cloud); std::swap(tempNormals, normals); if (mDebug) { std::cout << "Horizontal points remaining " << cloud->size() << std::endl; pcl::io::savePCDFileBinary("cloud.pcd", *cloud); pcl::io::savePCDFileBinary("robust_normals.pcd", *normals); } // plane segmentation t0 = std::chrono::high_resolution_clock::now(); if (mDebug) { std::cout << "segmenting planes..." << std::flush; } PlaneSegmenter segmenter; segmenter.setData(cloud, normals); segmenter.setMaxError(0.05); segmenter.setMaxAngle(5); segmenter.setMinPoints(100); PlaneSegmenter::Result segmenterResult = segmenter.go(); if (mDebug) { auto t1 = std::chrono::high_resolution_clock::now(); auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0); std::cout << "finished in " << dt.count()/1e3 << " sec" << std::endl; std::ofstream ofs("labels.txt"); for (const int lab : segmenterResult.mLabels) { ofs << lab << std::endl; } ofs.close(); ofs.open("planes.txt"); for (auto it : segmenterResult.mPlanes) { auto& plane = it.second; ofs << it.first << " " << plane.transpose() << std::endl; } ofs.close(); } // create point clouds std::unordered_map<int,std::vector<Eigen::Vector3f>> cloudMap; for (int i = 0; i < (int)segmenterResult.mLabels.size(); ++i) { int label = segmenterResult.mLabels[i]; if (label <= 0) continue; cloudMap[label].push_back(cloud->points[i].getVector3fMap()); } struct Plane { MatrixX3f mPoints; Eigen::Vector4f mPlane; }; std::vector<Plane> planes; planes.reserve(cloudMap.size()); for (auto it : cloudMap) { int n = it.second.size(); Plane plane; plane.mPoints.resize(n,3); for (int i = 0; i < n; ++i) plane.mPoints.row(i) = it.second[i]; plane.mPlane = segmenterResult.mPlanes[it.first]; planes.push_back(plane); } std::vector<RectangleFitter::Result> results; for (auto& plane : planes) { RectangleFitter fitter; fitter.setDimensions(mBlockDimensions.head<2>()); fitter.setAlgorithm((RectangleFitter::Algorithm)mRectangleFitAlgorithm); fitter.setData(plane.mPoints, plane.mPlane); auto result = fitter.go(); results.push_back(result); } if (mDebug) { std::ofstream ofs("boxes.txt"); for (int i = 0; i < (int)results.size(); ++i) { auto& res = results[i]; for (auto& p : res.mPolygon) { ofs << i << " " << p.transpose() << std::endl; } } ofs.close(); ofs.open("hulls.txt"); for (int i = 0; i < (int)results.size(); ++i) { auto& res = results[i]; for (auto& p : res.mConvexHull) { ofs << i << " " << p.transpose() << std::endl; } } ofs.close(); ofs.open("poses.txt"); for (int i = 0; i < (int)results.size(); ++i) { auto& res = results[i]; auto transform = res.mPose; ofs << transform.matrix() << std::endl; } ofs.close(); } for (int i = 0; i < (int)results.size(); ++i) { const auto& res = results[i]; if (mBlockDimensions.head<2>().norm() > 1e-5) { float areaRatio = mBlockDimensions.head<2>().prod()/res.mConvexArea; if ((areaRatio < mAreaThreshMin) || (areaRatio > mAreaThreshMax)) continue; } Block block; block.mSize << res.mSize[0], res.mSize[1], mBlockDimensions[2]; block.mPose = res.mPose; block.mPose.translation() -= block.mPose.rotation().col(2)*mBlockDimensions[2]/2; block.mHull = res.mConvexHull; result.mBlocks.push_back(block); } if (mDebug) { std::cout << "Surviving blocks: " << result.mBlocks.size() << std::endl; } result.mSuccess = true; return result; }
gtsam::Pose3 Convert(const Eigen::Isometry3f& iso) { gtsam::Matrix3 mat = iso.linear().cast<double>(); Eigen::Vector3d trans = iso.translation().cast<double>(); return gtsam::Pose3(mat, gtsam::Point3(trans)); }
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; }