/* ==================== TO IMPLEMENT ======================= * measurement(0) : x-position of marker in drone's xy-coordinate system (independant of roll, pitch) * measurement(1): y-position of marker in drone's xy-coordinate system (independant of roll, pitch) * measurement(2): yaw rotation of marker, in drone's xy-coordinate system (independant of roll, pitch) * * global_marker_pose(0): x-position or marker in world-coordinate system * global_marker_pose(1): y-position or marker in world-coordinate system * global_marker_pose(2): yaw-rotation or marker in world-coordinate system */ void ExtendedKalmanFilter::correctionStep(const Eigen::Vector3f& measurement, const Eigen::Vector3f& global_marker_pose) { printf("TO IMPLEMENT! ekf: %f %f %f; obs: %f %f %f\n", state[0],state[1],state[2], measurement[0], measurement[1], measurement[2]); float dx = global_marker_pose(0)-state(0); //dx = x_marker - x_drone_global float dy = global_marker_pose(1)-state(1);//dy = y_marker - y_drone_global float dYaw = global_marker_pose(2)-state(2);//dYaw = yaw_marker - yaw_drone_global // dh/dx: Eigen::Matrix3f H; H << -cos(state(2)), -sin(state(2)), -dx*sin(state(2))+dy*cos(state(2)), sin(state(2)), -cos(state(2)), -dx*cos(state(2))-dy*sin(state(2)), 0, 0, -1; //K Eigen::Matrix3f K, L; L = H*sigma*H.transpose()+R; K = sigma*H.transpose()*L.inverse(); //update state //f = z-h(x) Eigen::Vector3f f; f << measurement(0)-cos(state(2))*dx-sin(state(2))*dy, measurement(1)+sin(state(2))*dx-cos(state(2))*dy, measurement(2)-dYaw; f(2) = atan2(sin(f(2)),cos(f(2))); // normalize angle state = state + K*f; Eigen::Vector3f a; a = K*f; cout << "f: " << f << endl; cout << "K: " << K << endl; cout << "K*f" << a << endl; //update covariance matrix Eigen::Matrix3f I; I << 1,0,0, 0,1,0, 0,0,1; sigma = (I - K*H)*sigma; }
void CCubicGrids::extractRTFromBuffer(const cuda::GpuMat& cvgmSumBuf_, Eigen::Matrix3f* peimRw_, Eigen::Vector3f* peivTw_) const{ Mat cvmSumBuf; cvgmSumBuf_.download(cvmSumBuf); double* aHostTmp = (double*)cvmSumBuf.data; //declare A and b Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A; Eigen::Matrix<double, 6, 1> b; //retrieve A and b from cvmSumBuf short sShift = 0; for (int i = 0; i < 6; ++i){ // rows for (int j = i; j < 7; ++j) { // cols + b double value = aHostTmp[sShift++]; if (j == 6) // vector b b.data()[i] = value; else A.data()[j * 6 + i] = A.data()[i * 6 + j] = value; }//for each col }//for each row //checking nullspace double dDet = A.determinant(); if (fabs(dDet) < 1e-15 || dDet != dDet){ if (dDet != dDet) PRINTSTR("Failure -- dDet cannot be qnan. "); //reset (); return; }//if dDet is rational //float maxc = A.maxCoeff(); Eigen::Matrix<float, 6, 1> result = A.llt().solve(b).cast<float>(); //Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b); float alpha = result(0); float beta = result(1); float gamma = result(2); Eigen::Matrix3f Rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(beta, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX()); Eigen::Vector3f tinc = result.tail<3>(); //compose //eivTwCur = Rinc * eivTwCur + tinc; //eimrmRwCur = Rinc * eimrmRwCur; Eigen::Vector3f eivTinv = -peimRw_->transpose()* (*peivTw_); Eigen::Matrix3f eimRinv = peimRw_->transpose(); eivTinv = Rinc * eivTinv + tinc; eimRinv = Rinc * eimRinv; *peivTw_ = -eimRinv.transpose() * eivTinv; *peimRw_ = eimRinv.transpose(); }
void createSymmetricProblems ( size_t nr_problems, Vcl::Core::InterleavedArray<float, 3, 3, -1>& F, Vcl::Core::InterleavedArray<float, 3, 3, -1>* R ) { // Random number generator std::mt19937_64 rng; std::uniform_real_distribution<float> d; for (int i = 0; i < (int)nr_problems; i++) { // Rest-state Eigen::Matrix3f M; M << d(rng), d(rng), d(rng), d(rng), d(rng), d(rng), d(rng), d(rng), d(rng); Eigen::Matrix3f MtM = M.transpose() * M; F.at<float>(i) = MtM; if (R) { Eigen::Matrix3f Rot; Vcl::Mathematics::PolarDecomposition(MtM, Rot, nullptr); R->template at<float>(i) = Rot; } } }
void pcl::getCameraMatrixFromProjectionMatrix ( const Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, Eigen::Matrix3f& camera_matrix) { Eigen::Matrix3f KR = projection_matrix.topLeftCorner <3, 3> (); Eigen::Matrix3f KR_KRT = KR * KR.transpose (); Eigen::Matrix3f cam = KR_KRT / KR_KRT.coeff (8); memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9); camera_matrix.coeffRef (8) = 1.0; if (camera_matrix.Flags & Eigen::RowMajorBit) { camera_matrix.coeffRef (2) = cam.coeff (2); camera_matrix.coeffRef (5) = cam.coeff (5); camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5))); camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4); camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2))); } else { camera_matrix.coeffRef (6) = cam.coeff (2); camera_matrix.coeffRef (7) = cam.coeff (5); camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5))); camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4); camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2))); } }
void ShapeMatch::setTarget(std::vector<double> &target_vec) { target_mat = Eigen::Map<Eigen::Matrix3Xd>(&target_vec[0], 3, target_vec.size()/3); target_center = Eigen::Vector3d(target_mat.row(0).mean(), target_mat.row(0).mean(), target_mat.row(0).mean()); target_mat.row(0) = (target_mat.row(0).array() - target_center(0)).matrix(); target_mat.row(1) = (target_mat.row(1).array() - target_center(1)).matrix(); target_mat.row(2) = (target_mat.row(2).array() - target_center(2)).matrix(); // do icp alignment first //Eigen::MatrixXd C_n = Eigen::MatrixXd::Identity(target_mat.cols(), target_mat.cols()) - Eigen::MatrixXd::Ones(target_mat.cols(), target_mat.cols()) / target_mat.cols(); Eigen::Matrix3d H = target_mat*template_mat.transpose(); Eigen::Matrix3f H_float = H.cast<float>(); Eigen::Matrix3f Ui; Eigen::Vector3f Wi; Eigen::Matrix3f Vi; wunderSVD3x3(H_float, Ui, Wi, Vi); Eigen::Matrix3d R = (Vi * Ui.transpose()).cast<double>(); Eigen::Vector3d t = template_center - R*target_center; target_mat = R*target_mat + t*Eigen::MatrixXd::Ones(1, target_mat.cols()); target_center = Eigen::Vector3d(target_mat.row(0).mean(), target_mat.row(0).mean(), target_mat.row(0).mean()); target_mat.row(0) = (target_mat.row(0).array() - target_center(0)).matrix(); target_mat.row(1) = (target_mat.row(1).array() - target_center(1)).matrix(); target_mat.row(2) = (target_mat.row(2).array() - target_center(2)).matrix(); target_vec = std::vector<double>(target_mat.data(), target_mat.data()+target_mat.cols()*target_mat.rows()); }
/** Get the inliers among all matches that comply with a given fundamental matrix. * @param std::vector<Match> vector with feature matches * @param Eigen::Matrix3f fundamental matrix * @return std::vector<int> vector with indices of the inliers */ std::vector<int> MonoOdometer5::getInliers(std::vector<Match> matches, Eigen::Matrix3f F) { std::vector<int> inlierIndices; for(int i=0; i<matches.size(); i++) { cv::Point2f pPrev = matches[i].pPrev_; cv::Point2f pCurr = matches[i].pCurr_; Eigen::Vector3f pPrevHomog, pCurrHomog; pPrevHomog << pPrev.x, pPrev.y, 1.0; pCurrHomog << pCurr.x, pCurr.y, 1.0; // xCurr^T * F * xPrev double x2tFx1 = pCurrHomog.transpose() * F * pPrevHomog; // F * xPrev Eigen::Vector3f Fx1 = F * pPrevHomog; // F^T * xCurr Eigen::Vector3f Ftx2 = F.transpose() * pCurrHomog; // compute Sampson distance (distance to epipolar line) double dSampson = (x2tFx1 * x2tFx1) / ((Fx1(0)*Fx1(0)) + (Fx1(1)*Fx1(1)) + (Ftx2(0)*Ftx2(0)) + (Ftx2(1)*Ftx2(1))); if(dSampson < param_odometerInlierThreshold_) { inlierIndices.push_back(i); } } return inlierIndices; }
void PairwiseRegistrationDialog::showResults(const Eigen::Matrix4f &transformation, float rmsError, int corrNumber) { std::stringstream temp; Eigen::Matrix4f T = transformation; temp.str(""); temp << T; TTextEdit->setText(QString::fromStdString(temp.str())); Eigen::Matrix3f R = T.block(0,0,3,3); float c = (R * R.transpose()).diagonal().mean(); c = qSqrt(c); cLineEdit->setText(QString::number(c)); R /= c; temp.str(""); temp << R; RTextEdit->setText(QString::fromStdString(temp.str())); Eigen::Vector3f t = T.block(0,3,3,1); temp.str(""); temp << t; tLineEdit->setText(QString::fromStdString(temp.str())); Eigen::AngleAxisf angleAxis(R); angleLineEdit->setText( QString::number(angleAxis.angle()) ); temp.str(""); temp << angleAxis.axis(); axisLineEdit->setText(QString::fromStdString(temp.str())); corrNumberLineEdit->setText(QString::number(corrNumber)); errorLineEdit->setText(QString::number(rmsError)); }
template <typename PointSource, typename PointTarget> void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::getTransformationFromCorrelation ( const Eigen::MatrixXf &cloud_src_demean, const Eigen::Vector4f ¢roid_src, const Eigen::MatrixXf &cloud_tgt_demean, const Eigen::Vector4f ¢roid_tgt, Eigen::Matrix4f &transformation_matrix) { transformation_matrix.setIdentity (); // Assemble the correlation matrix H = source * target' Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>(); // Compute the Singular Value Decomposition Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3f u = svd.matrixU (); Eigen::Matrix3f v = svd.matrixV (); // Compute R = V * U' if (u.determinant () * v.determinant () < 0) { for (int x = 0; x < 3; ++x) v (x, 2) *= -1; } Eigen::Matrix3f R = v * u.transpose (); // Return the correct transformation transformation_matrix.topLeftCorner<3, 3> () = R; Eigen::Vector3f Rc = R * centroid_src.head<3> (); transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc; }
/* compute Jacobian */ void mmf::OptSO3ApproxGD::ComputeJacobian(const SO3f& thetaPrev, const SO3f& theta, Eigen::Vector3f* J, float* f) { Eigen::Matrix3f R = theta.matrix(); Eigen::Matrix3f Rprev = thetaPrev.matrix(); #ifndef NDEBUG cout<<"qKarch"<<endl<<qKarch_<<endl; cout<<"xSums_"<<endl<<xSums_<<endl; cout<<"Ns_"<<endl<<Ns_<<endl; #endif if(J) for (uint32_t k=0; k<3; ++k) (*J)(k) = tauR_*(Rprev.transpose()*SO3f::G(k)*R).trace(); if(f) *f = tauR_*(Rprev.transpose()*R).trace(); for (uint32_t j=0; j<6; ++j) { if (Ns_(j) == 0) continue; Eigen::Vector3f m = Eigen::Vector3f::Zero(); m(j/2) = j%2==0?-1.:1.; float dot = max(-1.0f,min(1.0f,qKarch_.col(j).dot(R*m))); float eps = acos(dot); if (f) *f += Ns_(j)*eps*eps; if (J) { float factor = 0.; if(-0.99< dot && dot < 0.99) factor = -(2.*Ns_(j)*eps)/(sqrt(1.f-dot*dot)); else if(dot >= 0.99) { // taylor series around 0.99 according to Mathematica factor = -2.*Ns_(j)*(1.0033467240646519 - 0.33601724502488395 *(-0.99 + dot) + 0.13506297338381046* (-0.99 + dot)*(-0.99 + dot)); }else if (dot <= -0.99) { factor = -2.*Ns_(j)*(21.266813135156017 - 1108.2484926534892*(0.99 + dot) + 83235.29739487475*(0.99 + dot)*(0.99 + dot)); } // std::cout << "factor " << factor << std::endl; for (uint32_t k=0; k<3; ++k) (*J)(k) += factor*qKarch_.col(j).dot(SO3f::G(k)*R*m); } } if (f) *f /= -Ns_.sum(); if (J) *J /= -Ns_.sum(); }
void ARAPTerm::updateRotation() { Solver::DeformPetal& deform_petal = Solver::deform_petals_[petal_id_]; Solver::PetalMatrix& origin_petal = deform_petal._origin_petal; Solver::PetalMatrix& petal_matrix = deform_petal._petal_matrix; Solver::RotList& rot_list = deform_petal._R_list; Solver::ScaleList& scale_list = deform_petal._S_list; Solver::AdjList& adj_list = deform_petal._adj_list; Solver::WeightMatrix& weight_matrix = deform_petal._weight_matrix; Eigen::Matrix3f Si; Eigen::MatrixXd Di; Eigen::Matrix3Xd Pi_Prime; Eigen::Matrix3Xd Pi; for (size_t i = 0, i_end = rot_list.size(); i < i_end; ++i) { Di = Eigen::MatrixXd::Zero(adj_list[i].size(), adj_list[i].size()); Pi_Prime.resize(3, adj_list[i].size()); Pi.resize(3, adj_list[i].size()); for (size_t j = 0, j_end = adj_list[i].size(); j < j_end; ++j) { Di(j, j) = weight_matrix.coeffRef(i, adj_list[i][j]); Pi.col(j) = origin_petal.col(i) - origin_petal.col(adj_list[i][j]); Pi_Prime.col(j) = petal_matrix.col(i) - petal_matrix.col(adj_list[i][j]); } Si = Pi.cast<float>() * Di.cast<float>() * Pi_Prime.transpose().cast<float>(); Eigen::Matrix3f Ui; Eigen::Vector3f Wi; Eigen::Matrix3f Vi; wunderSVD3x3(Si, Ui, Wi, Vi); rot_list[i] = Vi.cast<double>() * Ui.transpose().cast<double>(); if (rot_list[i].determinant() < 0) std::cout << "determinant is negative!" << std::endl; double s = 0; for (size_t j = 0, j_end = adj_list[i].size(); j < j_end; ++j) { s += Di(j, j) * Pi.col(j).squaredNorm(); } // scale_list[i] = Wi.trace() / s; /* if (scale_list[i] < 0.95 ) scale_list[i] = 0.95; else if (scale_list[i] > 1.05) scale_list[i] = 1.05;*/ } /*if (petal_id_ == 0) std::cout << "vertex: " << 0 << " " << scale_list[0] << std::endl;*/ }
void CGLUtil::viewerGL() { glMatrixMode(GL_MODELVIEW); // load the matrix to set camera pose glLoadMatrixf(_eimModelViewGL.data()); //rotation Eigen::Matrix3f eimRotation; if( btl::utility::BTL_GL == _eConvention ){ eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX()); // 3. rotate horizontally }//mouse x-movement is the rotation around y-axis else if( btl::utility::BTL_CV == _eConvention ) { eimRotation = Eigen::AngleAxisf(float(_dXAngle*M_PI/180.f), -Eigen::Vector3f::UnitY())* Eigen::AngleAxisf(float(_dYAngle*M_PI/180.f), Eigen::Vector3f::UnitX()); // 3. rotate horizontally } //translation /*_dZoom = _dZoom < 0.1? 0.1: _dZoom; _dZoom = _dZoom > 10? 10: _dZoom;*/ //get direction N pointing from camera center to the object centroid Eigen::Affine3f M; M = _eimModelViewGL; Eigen::Vector3f T = M.translation(); Eigen::Matrix3f R = M.linear(); Eigen::Vector3f C = -R.transpose()*T;//camera centre Eigen::Vector3f N = _eivCentroid - C;//from camera centre to object centroid N = N/N.norm();//normalization Eigen::Affine3f _eimManipulate; _eimManipulate.setIdentity(); _eimManipulate.translate( N*float(_dZoom) );//(N*(1-_dZoom)); //use camera movement toward object for zoom in/out effects _eimManipulate.translate(_eivCentroid); // 5. translate back to the original camera pose //_eimManipulate.scale(s); // 4. zoom in/out, never use scale to simulate camera movement, it affects z-buffer capturing. use translation instead _eimManipulate.rotate(eimRotation); // 2. rotate vertically // 3. rotate horizontally _eimManipulate.translate(-_eivCentroid); // 1. translate the camera center to align with object centroid*/ glMultMatrixf(_eimManipulate.data()); /* lTranslated( _aCentroid[0], _aCentroid[1], _aCentroid[2] ); // 5. translate back to the original camera pose _dZoom = _dZoom < 0.1? 0.1: _dZoom; _dZoom = _dZoom > 10? 10: _dZoom; glScaled( _dZoom, _dZoom, _dZoom ); // 4. zoom in/out, if( btl::utility::BTL_GL == _eConvention ) glRotated ( _dXAngle, 0, 1 ,0 ); // 3. rotate horizontally else if( btl::utility::BTL_CV == _eConvention ) //mouse x-movement is the rotation around y-axis glRotated ( _dXAngle, 0,-1 ,0 ); glRotated ( _dYAngle, 1, 0 ,0 ); // 2. rotate vertically glTranslated(-_aCentroid[0],-_aCentroid[1],-_aCentroid[2] ); // 1. translate the camera center to align with object centroid */ // light position in 3d glLightfv(GL_LIGHT0, GL_POSITION, _aLight); }
void CUDABuildLinearSystemRGBD::applyBL(CameraTrackingInput cameraTrackingInput, Eigen::Matrix3f& intrinsics, CameraTrackingParameters cameraTrackingParameters, float3 anglesOld, float3 translationOld, unsigned int imageWidth, unsigned int imageHeight, unsigned int level, Matrix6x7f& res, LinearSystemConfidence& conf) { unsigned int localWindowSize = 12; if(level != 0) localWindowSize = max(1, localWindowSize/(4*level)); const unsigned int blockSize = 64; const unsigned int dimX = (unsigned int)ceil(((float)imageWidth*imageHeight)/(localWindowSize*blockSize)); Eigen::Matrix3f intrinsicsRowMajor = intrinsics.transpose(); // Eigen is col major / cuda is row major computeNormalEquations(imageWidth, imageHeight, d_output, cameraTrackingInput, intrinsicsRowMajor.data(), cameraTrackingParameters, anglesOld, translationOld, localWindowSize, blockSize); cutilSafeCall(cudaMemcpy(h_output, d_output, sizeof(float)*30*dimX, cudaMemcpyDeviceToHost)); // Copy to CPU res = reductionSystemCPU(h_output, dimX, conf); }
template <typename PointT> void pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD ( const pcl::PointCloud<PointT> &cloud_src, const std::vector<int> &indices_src, const pcl::PointCloud<PointT> &cloud_tgt, const std::vector<int> &indices_tgt, Eigen::VectorXf &transform) { transform.resize (16); Eigen::Vector4f centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (cloud_src, indices_src, centroid_src); compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt); // Subtract the centroids from source, target Eigen::MatrixXf cloud_src_demean; demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean); Eigen::MatrixXf cloud_tgt_demean; demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean); // Assemble the correlation matrix H = source * target' Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>(); // Compute the Singular Value Decomposition Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3f u = svd.matrixU (); Eigen::Matrix3f v = svd.matrixV (); // Compute R = V * U' if (u.determinant () * v.determinant () < 0) { for (int x = 0; x < 3; ++x) v (x, 2) *= -1; } Eigen::Matrix3f R = v * u.transpose (); // Return the correct transformation transform.segment<3> (0) = R.row (0); transform[12] = 0; transform.segment<3> (4) = R.row (1); transform[13] = 0; transform.segment<3> (8) = R.row (2); transform[14] = 0; Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> (); transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0; }
/** * estimateRigidTransformationSVD */ void RigidTransformationRANSAC::estimateRigidTransformationSVD( const std::vector<Eigen::Vector3f > &srcPts, const std::vector<int> &srcIndices, const std::vector<Eigen::Vector3f > &tgtPts, const std::vector<int> &tgtIndices, Eigen::Matrix4f &transform) { Eigen::Vector3f srcCentroid, tgtCentroid; // compute the centroids of source, target ComputeCentroid (srcPts, srcIndices, srcCentroid); ComputeCentroid (tgtPts, tgtIndices, tgtCentroid); // Subtract the centroids from source, target Eigen::MatrixXf srcPtsDemean; DemeanPoints(srcPts, srcIndices, srcCentroid, srcPtsDemean); Eigen::MatrixXf tgtPtsDemean; DemeanPoints(tgtPts, tgtIndices, tgtCentroid, tgtPtsDemean); // Assemble the correlation matrix H = source * target' Eigen::Matrix3f H = (srcPtsDemean * tgtPtsDemean.transpose ()).topLeftCorner<3, 3>(); // Singular Value Decomposition Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3f u = svd.matrixU (); Eigen::Matrix3f v = svd.matrixV (); // Compute R = V * U' if (u.determinant () * v.determinant () < 0) { for (int x = 0; x < 3; ++x) v (x, 2) *= -1; } // Return the transformation Eigen::Matrix3f R = v * u.transpose (); Eigen::Vector3f t = tgtCentroid - R * srcCentroid; // set rotation transform.block(0,0,3,3) = R; // set translation transform.block(0,3,3,1) = t; transform.block(3, 0, 1, 3).setZero(); transform(3,3) = 1.; }
// odometry: // x: distance traveled in local x-direction // y: distance traveled in local y-direction // phi: rotation update void ExtendedKalmanFilter::predictionStep(const Eigen::Vector3f& odometry) { state(0) = state(0) + cos(state(2))*odometry(0) - sin(state(2))*odometry(1); state(1) = state(1) + sin(state(2))*odometry(0) + cos(state(2))*odometry(1); state(2) = state(2) + odometry(2); state(2) = atan2(sin(state(2)),cos(state(2))); // normalize angle // dg/dx: Eigen::Matrix3f G; G << 1, 0, -sin(state(2))*odometry(0) - cos(state(2))*odometry(1), 0, 1, cos(state(2))*odometry(0) - sin(state(2))*odometry(1), 0, 0, 1; sigma = G*sigma*G.transpose() + Q; }
void drawBoundingBox(PointCloudT::Ptr cloud, boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer, int z) { //Eigen::Vector4f centroid; pcl::compute3DCentroid(*cloud, centroid); //Eigen::Matrix3f covariance; computeCovarianceMatrixNormalized(*cloud, centroid, covariance); //Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, //Eigen::ComputeEigenvectors); eigen_solver.compute(covariance,Eigen::ComputeEigenvectors); // eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver> // (covariance,Eigen::ComputeEigenvectors); eigDx = eigen_solver.eigenvectors(); eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1)); //Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity()); p2w.block<3,3>(0,0) = eigDx.transpose(); p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>()); //pcl::PointCloud<PointT> cPoints; pcl::transformPointCloud(*cloud, cPoints, p2w); //PointT min_pt, max_pt; pcl::getMinMax3D(cPoints, min_pt, max_pt); const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap()); const Eigen::Quaternionf qfinal(eigDx); const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>(); //viewer->addPointCloud(cloud); viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200)); }
void PinholePointProjector::unProject(PointVector &points, Gaussian3fVector &gaussians, IntImage &indexImage, const DepthImage &depthImage) const { assert(depthImage.rows > 0 && depthImage.cols > 0 && "PinholePointProjector: Depth image has zero dimensions"); points.resize(depthImage.rows * depthImage.cols); gaussians.resize(depthImage.rows * depthImage.cols); indexImage.create(depthImage.rows, depthImage.cols); int count = 0; Point *point = &points[0]; Gaussian3f *gaussian = &gaussians[0]; float fB = _baseline * _cameraMatrix(0, 0); Eigen::Matrix3f J; for(int r = 0; r < depthImage.rows; r++) { const float *f = &depthImage(r, 0); int *i = &indexImage(r, 0); for(int c = 0; c < depthImage.cols; c++, f++, i++) { if(!_unProject(*point, c, r, *f)) { *i = -1; continue; } float z = *f; float zVariation = (_alpha * z * z) / (fB + z * _alpha); J << z, 0, (float)r, 0, z, (float)c, 0, 0, 1; J = _iK * J; Diagonal3f imageCovariance(3.0f, 3.0f, zVariation); Eigen::Matrix3f cov = J * imageCovariance * J.transpose(); *gaussian = Gaussian3f(point->head<3>(), cov); gaussian++; point++; *i = count; count++; } } points.resize(count); gaussians.resize(count); }
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; }
/// /// @par Detailed description /// ... /// @param [in, out] (param1) ... /// @return ... /// @note ... Eigen::Matrix3f sasmath::Math:: find_u(sasmol::SasMol &mol, int &frame) { Eigen::Matrix3f r ; float rxx = (mol._x().col(frame)*_x().col(frame)).sum() ; float rxy = (mol._x().col(frame)*_y().col(frame)).sum() ; float rxz = (mol._x().col(frame)*_z().col(frame)).sum() ; float ryx = (mol._y().col(frame)*_x().col(frame)).sum() ; float ryy = (mol._y().col(frame)*_y().col(frame)).sum() ; float ryz = (mol._y().col(frame)*_z().col(frame)).sum() ; float rzx = (mol._z().col(frame)*_x().col(frame)).sum() ; float rzy = (mol._z().col(frame)*_y().col(frame)).sum() ; float rzz = (mol._z().col(frame)*_z().col(frame)).sum() ; r << rxx,rxy,rxz, ryx,ryy,ryz, rzx,rzy,rzz; Eigen::Matrix3f rtr = r.transpose() * r ; Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(rtr); if (eigensolver.info() != Eigen::Success) { std::cout << "find_u calculation failed" << std::endl ; } Eigen::Matrix<float,3,1> uk ; // eigenvalues Eigen::Matrix3f ak ; // eigenvectors uk = eigensolver.eigenvalues() ; ak = eigensolver.eigenvectors() ; Eigen::Matrix3f akt = ak.transpose() ; Eigen::Matrix3f new_ak ; new_ak.row(0) = akt.row(2) ; //sort eigenvectors new_ak.row(1) = akt.row(1) ; new_ak.row(2) = akt.row(0) ; // python ak0 --> ak[2] ; ak1 --> ak[1] ; ak2 --> ak[0] //Eigen::Matrix<float,3,1> ak2 = akt.row(2).cross(akt.row(1)) ; Eigen::MatrixXf rak0 = (r * (akt.row(2).transpose())) ; Eigen::MatrixXf rak1 = (r * (akt.row(1).transpose())) ; Eigen::Matrix<float,3,1> urak0 ; if(uk[2] == 0.0) { urak0 = 1E15 * rak0 ; } else { urak0 = (1.0/sqrt(fabs(uk[2])))*rak0 ; } Eigen::Matrix<float,3,1> urak1 ; if(uk[1] == 0.0) { urak1 = 1E15 * rak1 ; } else { urak1 = (1.0/sqrt(fabs(uk[1])))*rak1 ; } Eigen::Matrix<float,3,1> urak2 = urak0.col(0).cross(urak1.col(0)) ; Eigen::Matrix3f bk ; bk << urak0,urak1,urak2; Eigen::Matrix3f u ; u = (bk * new_ak).transpose() ; return u ; /* u = array([[-0.94513198, 0.31068658, 0.100992 ], [-0.3006246 , -0.70612572, -0.64110165], [-0.12786863, -0.63628635, 0.76078203]]) check: u = -0.945133 0.310686 0.100992 -0.300624 -0.706126 -0.641102 -0.127869 -0.636287 0.760782 */ }
Eigen::VectorXf solve_ik(Eigen::Matrix3f goalR, Eigen::Vector3f goalT,int sol_index) { cout << "[SOLVER] Solution " << sol_index << std::endl; Eigen::VectorXf result(5); //~ index: //~ 0 direct space, low elbow //~ 1 direct space, high elbow //~ 2 inverse space, high elbow //~ 3 inverse space, low elbow Eigen::Matrix<float,5,4> DH; DH.row(0) << 0.0, 0.019, 0.0330, M_PI/2; DH.row(1) << 0.0, 0.0, 0.1550, 0.0; DH.row(2) << 0.0, 0.0, 0.1350, 0.0; DH.row(3) << 0.0, 0.0, 0.0, M_PI/2; DH.row(4) << 0.0, 0.2175, 0.0, 0.0; //~ from homing to DH structure //~ offset = [deg2rad(169) deg2rad(65+90) -deg2rad(146) deg2rad(102-90) deg2rad(167)]; Eigen::VectorXf offset(5); offset(0) = 169*DEG2RAD; offset(1) = 155*DEG2RAD; offset(2) = -146*DEG2RAD; offset(3) = 192*DEG2RAD; offset(4) = 167*DEG2RAD; float L2 = 0.155; float L3 = 0.135; float L4 = 0.2175; float beta = atan2(goalR(2,2), sqrt( pow(goalR(0,2),2) + pow(goalR(1,2),2) ) ); float q1, q2, q3, q4, q5, Z1, Z2, X1, X2; float sin_theta3, cos_theta3, theta3, theta2; if (sol_index == 0 || sol_index == 1) { //~ direct arm space q1 = atan2(goalT(1),goalT(0)); Z1 = goalT(2) - DH(0,1); Z2 = Z1 - L4*sin(beta); X1 = sqrt( pow(goalT(0),2) + pow(goalT(1),2) ) - DH(0,2); X2 = X1 - L4*cos(beta); } else { //~ inverse arm space q1 = atan2(goalT(1),goalT(0)) - M_PI; Z1 = goalT(2) - DH(0,1); Z2 = Z1 - L4*sin(beta); X1 = sqrt( pow(goalT(0),2) + pow(goalT(1),2) ) + DH(0,2); X2 = X1 - L4*cos(beta); } cos_theta3 = (-pow(Z2,2) -pow(X2,2) + pow(L2,2) + pow(L3,2)) / (2*L2*L3); if (cos_theta3 > -1 && cos_theta3 < 1) { // joint 3 if (sol_index == 0 || sol_index == 2) { sin_theta3 = sqrt(1-pow(cos_theta3,2)); theta3 = atan2(sin_theta3,cos_theta3); q3 = M_PI - theta3; } else { sin_theta3 = -sqrt(1-pow(cos_theta3,2)); theta3 = atan2(sin_theta3,cos_theta3); q3 = - M_PI - theta3; } // joint 2 float k1 = L2 + L3*cos(q3); float k2 = L3*sin(q3); if (sol_index == 0 || sol_index == 1) { theta2 = atan2(Z2,X2) - atan2(k2,k1); q2 = theta2; // joint 4 q4 = (M_PI/2) - (q2 + q3 - beta); } else { theta2 = atan2(Z2,X2) + atan2(k2,k1); q2 = M_PI - theta2; // joint 4 q4 = ( M_PI - (q2 + q3) - beta) + M_PI/2; } Eigen::MatrixXf T(4,4); T = Eigen::MatrixXf::Identity(4,4); Eigen::Vector4f joints(q1,q2,q3,q4); float j, d, a, alpha; for (int k=0; k<4; ++k) { j = joints(k); d = DH(k,1); a = DH(k,2); alpha = DH(k,3); // component transform matrix Eigen::Matrix<float,4,4> CTM; CTM.row(0) << cos(j), -sin(j)*cos(alpha), sin(j)*sin(alpha), a*cos(j); CTM.row(1) << sin(j), cos(j)*cos(alpha), -cos(j)*sin(alpha), a*sin(j); CTM.row(2) << 0.0, sin(alpha), cos(alpha), d; CTM.row(3) << 0.0, 0.0, 0.0, 1; T = T * CTM; } Eigen::Matrix3f tool = T.block(0,0,3,3); Eigen::Matrix3f roll = tool.transpose() * goalR; q5 = atan2(roll(1,0),roll(0,0)); T = Eigen::MatrixXf::Identity(4,4); //~ The setpoint angle for joint arm_joint_1 is out of range. The valid range is between 0.0100692 rad and 5.84014 rad //~ The setpoint angle for joint arm_joint_2 is out of range. The valid range is between 0.0100692 rad and 2.61799 rad //~ The setpoint angle for joint arm_joint_3 is out of range. The valid range is between -5.02655 rad and -0.015708 rad //~ The setpoint angle for joint arm_joint_4 is out of range. The valid range is between 0.0221239 rad and 3.4292 rad //~ The setpoint angle for joint arm_joint_5 is out of range. The valid range is between 0.110619 rad and 5.64159 rad result(0) = offset(0)-q1; result(1) = offset(1)-q2; result(2) = offset(2)-q3; result(3) = offset(3)-q4; result(4) = offset(4)-q5; if (result(4) < 0.110619 ) { result(4) = result(4) + M_PI; } else if ( result(4) > 5.64159) { result(4) = result(4) - M_PI; } } else { // position not reachable: arm will stand still (we will give impossible values for the default controller) result = result.setConstant(-10); } Eigen::Matrix<float,5,2> range; range.row(0) << 0.0100692+0.0017, 5.84014-0.0017; range.row(1) << 0.0100692+0.0017, 2.61799-0.0017; range.row(2) << -5.02655+0.0017, -0.015708-0.0017; range.row(3) << 0.0221239+0.0017, 3.4292-0.0017; range.row(4) << 0.110619+0.0017, 5.64159-0.0017; for(int k=0; k<5; ++k) { if( result(k) == -10) { cout << "[SOLVER] Solution " << sol_index << " discarded: desired goal not reachable by the robot" << std::endl; result = result.setZero(); k = 5; } else if( range(k,0) > result(k) || range(k,1) < result(k) ) { cout << "[SOLVER] Solution " << sol_index << " discarded: joint " << k+1 << " outside accetable ranges. Value: " << result(k) << "; Range: [" << range(k,0) << " , " << range(k,1) << "]" << std::endl; result = result.setZero(); k = 5; } else { } } cout << "----------" << std::endl; return result; }
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; }
void LocalRecognitionPipeline<PointT>::correspondenceGrouping () { if(cg_algorithm_->getRequiresNormals() && (!scene_normals_ || scene_normals_->points.size() != scene_->points.size())) computeNormals<PointT>(scene_, scene_normals_, param_.normal_computation_method_); typename std::map<std::string, ObjectHypothesis<PointT> >::iterator it; for (it = obj_hypotheses_.begin (); it != obj_hypotheses_.end (); it++) { ObjectHypothesis<PointT> &oh = it->second; if(oh.model_scene_corresp_.size() < 3) continue; std::vector < pcl::Correspondences > corresp_clusters; cg_algorithm_->setSceneCloud (scene_); cg_algorithm_->setInputCloud (oh.model_->keypoints_); if(cg_algorithm_->getRequiresNormals()) cg_algorithm_->setInputAndSceneNormals(oh.model_->kp_normals_, scene_normals_); //we need to pass the keypoints_pointcloud and the specific object hypothesis cg_algorithm_->setModelSceneCorrespondences (oh.model_scene_corresp_); cg_algorithm_->cluster (corresp_clusters); std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > new_transforms (corresp_clusters.size()); typename pcl::registration::TransformationEstimationSVD < PointT, PointT > t_est; for (size_t i = 0; i < corresp_clusters.size(); i++) t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, corresp_clusters[i], new_transforms[i]); if(param_.merge_close_hypotheses_) { std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > merged_transforms (corresp_clusters.size()); std::vector<bool> cluster_has_been_taken(corresp_clusters.size(), false); const double angle_thresh_rad = param_.merge_close_hypotheses_angle_ * M_PI / 180.f ; size_t kept=0; for (size_t i = 0; i < new_transforms.size(); i++) { if (cluster_has_been_taken[i]) continue; cluster_has_been_taken[i] = true; const Eigen::Vector3f centroid1 = new_transforms[i].block<3, 1> (0, 3); const Eigen::Matrix3f rot1 = new_transforms[i].block<3, 3> (0, 0); pcl::Correspondences merged_corrs = corresp_clusters[i]; for(size_t j=i; j < new_transforms.size(); j++) { const Eigen::Vector3f centroid2 = new_transforms[j].block<3, 1> (0, 3); const Eigen::Matrix3f rot2 = new_transforms[j].block<3, 3> (0, 0); const Eigen::Matrix3f rot_diff = rot2 * rot1.transpose(); double rotx = atan2(rot_diff(2,1), rot_diff(2,2)); double roty = atan2(-rot_diff(2,0), sqrt(rot_diff(2,1) * rot_diff(2,1) + rot_diff(2,2) * rot_diff(2,2))); double rotz = atan2(rot_diff(1,0), rot_diff(0,0)); double dist = (centroid1 - centroid2).norm(); if ( (dist < param_.merge_close_hypotheses_dist_) && (rotx < angle_thresh_rad) && (roty < angle_thresh_rad) && (rotz < angle_thresh_rad) ) { merged_corrs.insert( merged_corrs.end(), corresp_clusters[j].begin(), corresp_clusters[j].end() ); cluster_has_been_taken[j] = true; } } t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, merged_corrs, merged_transforms[kept]); kept++; } merged_transforms.resize(kept); new_transforms = merged_transforms; } std::cout << "Merged " << corresp_clusters.size() << " clusters into " << new_transforms.size() << " clusters. Total correspondences: " << oh.model_scene_corresp_.size () << " " << it->first << std::endl; // oh.visualize(*scene_); size_t existing_hypotheses = models_.size(); models_.resize( existing_hypotheses + new_transforms.size(), oh.model_ ); transforms_.insert(transforms_.end(), new_transforms.begin(), new_transforms.end()); } }
template<template<class > class Distance, typename PointT, typename FeatureT> bool GlobalNNPipelineROS<Distance,PointT,FeatureT>::classifyROS (classifier_srv_definitions::classify::Request & req, classifier_srv_definitions::classify::Response & response) { typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>()); pcl::fromROSMsg(req.cloud, *cloud); this->input_ = cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr pClusteredPCl (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::copyPointCloud(*cloud, *pClusteredPCl); //clear all data from previous visualization steps and publish empty markers/point cloud for (size_t i=0; i < markerArray_.markers.size(); i++) { markerArray_.markers[i].action = visualization_msgs::Marker::DELETE; } vis_pub_.publish( markerArray_ ); sensor_msgs::PointCloud2 scenePc2; vis_pc_pub_.publish(scenePc2); markerArray_.markers.clear(); for(size_t cluster_id=0; cluster_id < req.clusters_indices.size(); cluster_id++) { std::vector<int> cluster_indices_int; Eigen::Vector4f centroid; Eigen::Matrix3f covariance_matrix; const float r = std::rand() % 255; const float g = std::rand() % 255; const float b = std::rand() % 255; this->indices_.resize(req.clusters_indices[cluster_id].data.size()); for(size_t kk=0; kk < req.clusters_indices[cluster_id].data.size(); kk++) { this->indices_[kk] = static_cast<int>(req.clusters_indices[cluster_id].data[kk]); pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).r = 0.8*r; pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).g = 0.8*g; pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).b = 0.8*b; } this->classify (); std::cout << "for cluster " << cluster_id << " with size " << cluster_indices_int.size() << ", I have following hypotheses: " << std::endl; object_perception_msgs::classification class_tmp; for(size_t kk=0; kk < this->categories_.size(); kk++) { std::cout << this->categories_[kk] << " with confidence " << this->confidences_[kk] << std::endl; std_msgs::String str_tmp; str_tmp.data = this->categories_[kk]; class_tmp.class_type.push_back(str_tmp); class_tmp.confidence.push_back( this->confidences_[kk] ); } response.class_results.push_back(class_tmp); typename pcl::PointCloud<PointT>::Ptr pClusterPCl_transformed (new pcl::PointCloud<PointT>()); pcl::computeMeanAndCovarianceMatrix(*this->input_, cluster_indices_int, covariance_matrix, centroid); Eigen::Matrix3f eigvects; Eigen::Vector3f eigvals; pcl::eigen33(covariance_matrix, eigvects, eigvals); Eigen::Vector3f centroid_transformed = eigvects.transpose() * centroid.topRows(3); Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero(4,4); transformation_matrix.block<3,3>(0,0) = eigvects.transpose(); transformation_matrix.block<3,1>(0,3) = -centroid_transformed; transformation_matrix(3,3) = 1; pcl::transformPointCloud(*this->input_, cluster_indices_int, *pClusterPCl_transformed, transformation_matrix); //pcl::transformPointCloud(*frame_, cluster_indices_int, *frame_eigencoordinates_, eigvects); PointT min_pt, max_pt; pcl::getMinMax3D(*pClusterPCl_transformed, min_pt, max_pt); std::cout << "Elongations along eigenvectors: " << max_pt.x - min_pt.x << ", " << max_pt.y - min_pt.y << ", " << max_pt.z - min_pt.z << std::endl; geometry_msgs::Point32 centroid_ros; centroid_ros.x = centroid[0]; centroid_ros.y = centroid[1]; centroid_ros.z = centroid[2]; response.centroid.push_back(centroid_ros); // calculating the bounding box of the cluster Eigen::Vector4f min; Eigen::Vector4f max; pcl::getMinMax3D (*this->input_, cluster_indices_int, min, max); object_perception_msgs::BBox bbox; geometry_msgs::Point32 pt; pt.x = min[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt); pt.x = min[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt); pt.x = min[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt); pt.x = min[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt); pt.x = max[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt); pt.x = max[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt); pt.x = max[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt); pt.x = max[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt); response.bbox.push_back(bbox); // getting the point cloud of the cluster typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>); pcl::copyPointCloud(*this->input_, cluster_indices_int, *cluster); sensor_msgs::PointCloud2 pc2; pcl::toROSMsg (*cluster, pc2); response.cloud.push_back(pc2); // visualize the result as ROS topic visualization_msgs::Marker marker; marker.header.frame_id = camera_frame_; marker.header.stamp = ros::Time::now(); //marker.header.seq = ++marker_seq_; marker.ns = "object_classification"; marker.id = cluster_id; marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; marker.action = visualization_msgs::Marker::ADD; marker.pose.position.x = centroid_ros.x; marker.pose.position.y = centroid_ros.y - 0.1; marker.pose.position.z = centroid_ros.z - 0.1; marker.pose.orientation.x = 0; marker.pose.orientation.y = 0; marker.pose.orientation.z = 0; marker.pose.orientation.w = 1.0; marker.scale.z = 0.1; marker.color.a = 1.0; marker.color.r = r/255.f; marker.color.g = g/255.f; marker.color.b = b/255.f; std::stringstream marker_text; marker_text.precision(2); marker_text << this->categories_[0] << this->confidences_[0]; marker.text = marker_text.str(); markerArray_.markers.push_back(marker); } pcl::toROSMsg (*pClusteredPCl, scenePc2); vis_pc_pub_.publish(scenePc2); vis_pub_.publish( markerArray_ ); response.clusters_indices = req.clusters_indices; return true; }
void NMBasedCloudIntegration<PointT>::collectInfo () { size_t total_point_count = 0; for(size_t i = 0; i < input_clouds_.size(); i++) total_point_count += (indices_.empty() || indices_[i].empty()) ? input_clouds_[i]->size() : indices_[i].size(); VLOG(1) << "Allocating memory for point information of " << total_point_count << "points. "; big_cloud_info_.resize(total_point_count); std::vector<pcl::PointCloud<PointT> > input_clouds_aligned (input_clouds_.size()); std::vector<pcl::PointCloud<pcl::Normal> > input_normals_aligned (input_clouds_.size()); #pragma omp parallel for schedule(dynamic) for(size_t i=0; i < input_clouds_.size(); i++) { pcl::transformPointCloud(*input_clouds_[i], input_clouds_aligned[i], transformations_to_global_[i]); transformNormals(*input_normals_[i], input_normals_aligned[i], transformations_to_global_[i]); } size_t point_count = 0; for(size_t i=0; i < input_clouds_.size(); i++) { const pcl::PointCloud<PointT> &cloud_aligned = input_clouds_aligned[i]; const pcl::PointCloud<pcl::Normal> &normals_aligned = input_normals_aligned[i]; size_t kept_new_pts = 0; if (indices_.empty() || indices_[i].empty()) { for(size_t jj=0; jj<cloud_aligned.points.size(); jj++) { if ( !pcl::isFinite(cloud_aligned.points[jj]) || !pcl::isFinite(normals_aligned.points[jj]) ) continue; PointInfo &pt = big_cloud_info_[point_count + kept_new_pts]; pt.pt = cloud_aligned.points[jj]; pt.normal = normals_aligned.points[jj]; pt.sigma_lateral = pt_properties_[i][jj][0]; pt.sigma_axial = pt_properties_[i][jj][1]; pt.distance_to_depth_discontinuity = pt_properties_[i][jj][2]; pt.pt_idx = jj; kept_new_pts++; } } else { for(int idx : indices_[i]) { if ( !pcl::isFinite(cloud_aligned.points[idx]) || !pcl::isFinite(normals_aligned.points[idx]) ) continue; PointInfo &pt = big_cloud_info_[point_count + kept_new_pts]; pt.pt = cloud_aligned.points[idx]; pt.normal = normals_aligned.points[ idx ]; pt.sigma_lateral = pt_properties_[i][idx][0]; pt.sigma_axial = pt_properties_[i][idx][1]; pt.distance_to_depth_discontinuity = pt_properties_[i][idx][2]; pt.pt_idx = idx; kept_new_pts++; } } // compute and store remaining information #pragma omp parallel for schedule (dynamic) firstprivate(i, point_count, kept_new_pts) for(size_t jj=0; jj<kept_new_pts; jj++) { PointInfo &pt = big_cloud_info_ [point_count + jj]; pt.origin = i; Eigen::Matrix3f sigma = Eigen::Matrix3f::Zero(), sigma_aligned = Eigen::Matrix3f::Zero(); sigma(0,0) = pt.sigma_lateral; sigma(1,1) = pt.sigma_lateral; sigma(2,2) = pt.sigma_axial; const Eigen::Matrix4f &tf = transformations_to_global_[ i ]; Eigen::Matrix3f rotation = tf.block<3,3>(0,0); // or inverse? sigma_aligned = rotation * sigma * rotation.transpose(); double det = sigma_aligned.determinant(); // if( std::isfinite(det) && det>0) // pt.probability = 1 / sqrt(2 * M_PI * det); // else // pt.probability = std::numeric_limits<float>::min(); if( std::isfinite(det) && det>0) pt.weight = det; else pt.weight = std::numeric_limits<float>::max(); } point_count += kept_new_pts; } big_cloud_info_.resize(point_count); }
int main(int argc, char** argv) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>); if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file { PCL_ERROR("Couldn't read file test_pcd.pcd \n"); return (-1); } pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZRGB> seg; // Optional seg.setOptimizeCoefficients(true); // Mandatory seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.01); seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); if (inliers->indices.size() == 0) { PCL_ERROR("Could not estimate a planar model for the given dataset."); return (-1); } std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud(cloud); extract.setIndices(inliers); extract.setNegative(true); //Removes part_of_cloud but retain the original full_cloud //extract.setNegative(true); // Removes part_of_cloud from full cloud and keep the rest extract.filter(*cloud_plane); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud(cloud_plane); sor.setMeanK(50); sor.setStddevMulThresh(1.0); sor.filter(*cloud_filtered); //cloud.swap(cloud_plane); /* pcl::visualization::CloudViewer viewer("Simple Cloud Viewer"); viewer.showCloud(cloud_plane); while (!viewer.wasStopped()) { } */ Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity(); Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis; fitted_plane_norm[0] = coefficients->values[0]; fitted_plane_norm[1] = coefficients->values[1]; fitted_plane_norm[2] = coefficients->values[2]; xyaxis_plane_norm[0] = 0.0; xyaxis_plane_norm[1] = 0.0; xyaxis_plane_norm[2] = 1.0; rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm); float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm)); //float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm)); transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis)); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud(*cloud_recentered, *cloud_xyz); /////////////////////////////////////////////////////////////////// Eigen::Vector4f pcaCentroid; pcl::compute3DCentroid(*cloud_xyz, pcaCentroid); Eigen::Matrix3f covariance; computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors); Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors(); std::cout << eigenVectorsPCA.size() << std::endl; if(eigenVectorsPCA.size()!=9) eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity()); projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>()); pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform); // Get the minimum and maximum points of the transformed cloud. pcl::PointXYZ minPoint, maxPoint; pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint); const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap()); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer = rgbVis(cloud); // viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud"); // viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2"); viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z); while (!viewer->wasStopped()) { viewer->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } /* pcl::PCA< pcl::PointXYZ > pca; pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud; pcl::PointCloud< pcl::PointXYZ > proj; pca.setInputCloud(cloud); pca.project(*cloud, proj); Eigen::Vector4f proj_min; Eigen::Vector4f proj_max; pcl::getMinMax3D(proj, proj_min, proj_max); pcl::PointXYZ min; pcl::PointXYZ max; pca.reconstruct(proj_min, min); pca.reconstruct(proj_max, max); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z); */ return (0); }
int main(int argc, char** argv) { // Punktwolke laden pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_unfiltered (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile <pcl::PointXYZ> ("Arena_24_7_2014.pcd",*cloud_unfiltered) == -1) { std::cout<<"Cloud reading failed"<<std::endl; return (-1); } pcl::PCDWriter writer; //Filter für Höhen- und Tiefenbegrenzung: pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud(cloud_unfiltered); pass.setFilterFieldName("z"); pass.setFilterLimits(-0.2,0.3);//Für Stativ ca. (-0.9,-0.2) pass.filter(*cloud); // Berechnen der Punktnormalen pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>); pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>); pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator; normal_estimator.setSearchMethod(tree); normal_estimator.setInputCloud(cloud); normal_estimator.setKSearch(100);// Ursprünglich:50 // http://pointclouds.org/documentation/tutorials/normal_estimation.php normal_estimator.compute(*normals); // Segmentierung mit dem RegionGrowing-Algorithmus (Setzen der Parameter) pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg; reg.setMinClusterSize (200); //reg.setMaxClusterSize (10000); reg.setSearchMethod (tree); reg.setNumberOfNeighbours (50); // Ursprünglich 50 (Bei ca. 150 werden die Kanten schön glatt!) // aber es wird nach zu vielen Nachbarn gecheckt-> Mehrere Banden fallen in ein Cluster reg.setInputCloud (cloud); //reg.setIndices (indices); reg.setInputNormals (normals); reg.setSmoothnessThreshold (4.0 / 180.0 * M_PI); // Ursprünglich: 7.0/180*M_PI // je kleiner desto weniger punkte sind "smooth" genug ->Klares Abgrenzen der Cluster! reg.setCurvatureThreshold (.3);//Ursprünglich:1.0 // je kleiner desto geringer darf sich das Cluster krümmen // Anwendung des Cluster-Filters auf Input-Wolke "cloud" std::vector <pcl::PointIndices> clusters; reg.extract (clusters); pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr planes_cloud (new pcl::PointCloud<pcl::PointXYZ>); //vtkSmartPointer<vtkAppendPolyData> appendFilter = //vtkSmartPointer<vtkAppendPolyData>::New(); //vtkSmartPointer<vtkPolyData> polyplane = //vtkSmartPointer<vtkPolyData>::New(); int b=0; // Schleife über alle detektierten Cluster for(int a=0;a<clusters.size();a++) { if(clusters[a].indices.size() >100 ) { pcl::IndicesPtr indices_ptr (new std::vector<int> (clusters[a].indices.size ())); for (int i=0;i<indices_ptr->size();i++) { (*indices_ptr)[i]=clusters[a].indices[i]; // http://www.pcl-users.org/Removing-a-cluster-Problem-with-pointer-td4023699.html } // Indizes des jeweiligen Clusters werden alle in indices_ptr gespeichert // Punkte des Clusters werden in cluster_cloud geschrieben extract.setIndices(indices_ptr); extract.setNegative(false); extract.filter(*cluster_cloud);// Punkte des Cluster a werden in cluster_cloud geschrieben std::cout<<"cluster_cloud["<<a<<"] hat "<<cluster_cloud->width*cluster_cloud->height<<" Punkte."<<std::endl; //Erzeugen einer Ebene aus Cluster_cloud pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); pcl::SACSegmentation<pcl::PointXYZ> seg; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(1000); seg.setDistanceThreshold(0.1); seg.setInputCloud(cluster_cloud); seg.segment(*inliers, *coefficients); // Wenn Ebene vertikal: Abspeichern in Cluster_i.pcd if(coefficients->values[2]<.9 && coefficients->values[2]>(-.9))//ax+by+cz+d=0 (wenn c=0 => Ebene parallel zur z-Achse) { pcl::PointCloud<pcl::PointXYZ>::Ptr planes_projected (new pcl::PointCloud<pcl::PointXYZ>); //Inliers auf Ebene projizieren: pcl::PCA<pcl::PointXYZ> pca2; pcl::ProjectInliers<pcl::PointXYZ> proj2; proj2.setModelType(pcl::SACMODEL_PLANE); proj2.setIndices(inliers); proj2.setInputCloud(cluster_cloud); proj2.setModelCoefficients(coefficients); proj2.filter(*planes_projected); // Punkte in Eigenraum transformieren, um bounding box zu berechnen (Quelle: pcl-users Forum (http://www.pcl-users.org/Finding-oriented-bounding-box-of-a-cloud-td4024616.html) // compute principal direction Eigen::Vector4f centroid; pcl::compute3DCentroid(*planes_projected, centroid); Eigen::Matrix3f covariance; computeCovarianceMatrixNormalized(*planes_projected, centroid, covariance); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors); Eigen::Matrix3f eigDx = eigen_solver.eigenvectors(); eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1)); // move the points to the that reference frame Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity()); p2w.block<3,3>(0,0) = eigDx.transpose(); p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>()); pcl::PointCloud<pcl::PointXYZ> cPoints; pcl::transformPointCloud(*planes_projected, cPoints, p2w); pcl::PointXYZ min_pt, max_pt; pcl::getMinMax3D(cPoints, min_pt, max_pt); const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap()); // final transform const Eigen::Quaternionf qfinal(eigDx); const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>(); // Punktwolke und bounding box im viewer anzeigen boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer); viewer->addCoordinateSystem (); viewer->addPointCloud(planes_projected);// Danach auskommentieren viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z); // Ausgabe der Eckpunkte (kann gelöscht werden) std::cout << " min.x= " << min_pt.x << " max.x= " << max_pt.x << " min.y= " << min_pt.y << " max.y= " << max_pt.y << " min.z= " << min_pt.z << " max.z= " << max_pt.z<< std::endl; std::cout << "Punkte: " << min_pt.x <<";" << min_pt.y << ";" << min_pt.z <<std::endl; std::cout << min_pt.x <<";" << min_pt.y << ";" << max_pt.z <<std::endl; std::cout << min_pt.x <<";" << max_pt.y << ";" << min_pt.z <<std::endl; std::cout << min_pt.x <<";" << max_pt.y << ";" << max_pt.z <<std::endl; std::cout << max_pt.x <<";" << min_pt.y << ";" << min_pt.z <<std::endl; std::cout << max_pt.x <<";" << min_pt.y << ";" << max_pt.z <<std::endl; std::cout << max_pt.x <<";" << max_pt.y << ";" << min_pt.z <<std::endl; std::cout << max_pt.x <<";" << max_pt.y << ";" << max_pt.z <<std::endl; // Erzeugen einer Punktwolke mit Punkten, die zwischen den Extrema liegen (um anschließend eine dichte Oberfläche zu erzeugen) pcl::PointCloud<pcl::PointXYZ>::Ptr Fillcloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointXYZ P1,P2,P3,P4; P1.x=max_pt.x;P2.x=max_pt.x; P3.x=max_pt.x; P4.x=max_pt.x; P1.y=min_pt.y;P2.y=min_pt.y; P3.y=max_pt.y; P4.y=max_pt.y; P1.z=min_pt.z;P2.z=max_pt.z; P3.z=min_pt.z; P4.z=max_pt.z; //P4.x=min_pt.x;P3.x=min_pt.x; P2.x=min_pt.x; P1.x=min_pt.x; //P4.y=min_pt.y;P3.y=min_pt.y; P2.y=max_pt.y; P1.y=max_pt.y; //P4.z=min_pt.z;P3.z=max_pt.z; P2.z=min_pt.z; P1.z=max_pt.z; Fillcloud->push_back(P1); Fillcloud->push_back(P2); Fillcloud->push_back(P3); Fillcloud->push_back(P4); // Fillcloud->push_back (pcl::PointXYZ (min_pt.x, min_pt.y, min_pt.z)); // P1 // Fillcloud->push_back (pcl::PointXYZ (min_pt.x, min_pt.y, max_pt.z)); // P2 // Fillcloud->push_back (pcl::PointXYZ (min_pt.x, max_pt.y, min_pt.z)); // P3 // Fillcloud->push_back (pcl::PointXYZ (min_pt.x, max_pt.y, max_pt.z)); // P4 // Enweder alle x_max oder alle x_min nehmen WARUM????? // Fillcloud->push_back (pcl::PointXYZ (max_pt.x, min_pt.y, min_pt.z)); // Fillcloud->push_back (pcl::PointXYZ (max_pt.x, min_pt.y, max_pt.z)); // Fillcloud->push_back (pcl::PointXYZ (max_pt.x, max_pt.y, min_pt.z)); // Fillcloud->push_back (pcl::PointXYZ (max_pt.x, max_pt.y, max_pt.z)); // Schleife, um BoundingBox-"Fläche" mit Punkten zu füllen (um ein Mesh erzeugen zu können int AnzahlPunktehoch = 80; int AnzahlPunktebreit = 400; for( int ii = 0; ii < AnzahlPunktebreit+1; ii++ ){ Fillcloud->push_back( pcl::PointXYZ (P1.x+((P2.x-P1.x)/AnzahlPunktebreit)*ii , P1.y+((P2.y-P1.y)/AnzahlPunktebreit)*ii , P1.z+((P2.z-P1.z)/AnzahlPunktebreit)*ii) ); for( int jj = 0; jj < AnzahlPunktehoch+1; jj++ ){ Fillcloud->push_back( pcl::PointXYZ (P1.x+((P2.x-P1.x)/AnzahlPunktebreit)*ii + (P3.x-P1.x)/AnzahlPunktehoch*jj , P1.y+((P2.y-P1.y)/AnzahlPunktebreit)*ii + (P3.y-P1.y)/AnzahlPunktehoch*jj , P1.z+((P2.z-P1.z)/AnzahlPunktebreit)*ii + (P3.z-P1.z)/AnzahlPunktehoch*jj)); } } // Erzeugte Punktwolke, die die Extrema "verbindet" rücktransformieren pcl::PointCloud<pcl::PointXYZ>::Ptr Fillcloud_transformiert (new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*Fillcloud,*Fillcloud_transformiert,tfinal,qfinal); // Fillcloud wird zum Viewer hinzugefügt; "spin" auskommentieren wenn nicht jedes Fenster einzeln weggeklickt werden soll //viewer->addPointCloud(Fillcloud_transformiert); //viewer->spin(); //Alle Clusterebenen, die vertikal sind werden in planes_cloud gespeichert *planes_cloud+=*Fillcloud_transformiert; // Aus aktuellen Punkten vtkPlaneSource erzeugen vtkPlaneSource *plane= vtkPlaneSource::New(); vtkPlaneSource *plane2= vtkPlaneSource::New(); plane->SetOrigin(min_pt.x,min_pt.y,min_pt.z); plane->SetPoint1(min_pt.x,min_pt.y,max_pt.z); plane->SetPoint2(min_pt.x,max_pt.y,min_pt.z); plane->SetXResolution(10); //plane->SetResolution(10,200); // Set the number of x-y subdivisions in the plane plane->Update(); plane2->SetOrigin(min_pt.x,min_pt.y,max_pt.z); plane2->SetPoint1(min_pt.x,max_pt.y,max_pt.z); plane2->SetPoint2(min_pt.x,max_pt.y,min_pt.z); //plane->SetResolution(1,1); plane2->Update(); // appender->AddInputConnection(plane->GetOutputPort()); // appender->Update(); // actor->SetMapper(mapper); // // Aktuelle Plane zu PlaneCollection hinzufügen // PlaneCollection->AddItem(plane->GetOutputPort()); // PlaneCollection->AddItem(plane); // mapper->SetInputConnection(plane->GetOutputPort()); // plane->Delete(); // mapper->SetInputConnection(plane->GetOutputPort()); // actor->SetMapper(mapper); // renderer->AddActor(plane); // vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New(); // polydata->SetVerts(); // polyplane->ShallowCopy(plane->GetOutput()); // appendFilter->AddInputConnection(polyplane->GetProducerPort()); //appendFilter->AddInput(plane->GetOutput()); //appendFilter->Update(); //appendFilter->AddInput(plane2->GetOutput()); //appendFilter->Update(); b=b+1; } } } // PlanesCollection als STL abspeichern //vtkSmartPointer<vtkSTLWriter> schreiber = vtkSmartPointer<vtkSTLWriter>::New(); //schreiber->SetInput(appendFilter->GetOutput()); //schreiber->SetFileName("funktioniert nicht!"); //schreiber->Update(); //schreiber->Write(); // schreiber->SetInputConnection(plane->GetOutputPort()); // schreiber->SetFileName(""); // schreiber->SetInput(appender->GetOutput()); // //schreiber->SetFileTypeToASCII(); // schreiber->Write(); // Ausgabe std::cout<<"Es wurden "<<b<<" Flächen in Planes_cloud.pcd geschrieben"<<endl; writer.write("Planes_cloud.pcd",*planes_cloud,false); std::cout << "Number of clusters is equal to " << clusters.size () << std::endl; std::cout << "First cluster has " << clusters[0].indices.size () << " points." << endl; std::cout << "These are the indices of the points of the initial" << std::endl << "cloud that belong to the first cluster:" << std::endl; int counter = 0; while (counter < 5 || counter > clusters[0].indices.size ()) { std::cout << clusters[0].indices[counter] << std::endl; counter++; } // planes_cloud zu einer Oberfläche konvertieren u. als stl oder vtk speichern! pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n; pcl::PointCloud<pcl::Normal>::Ptr normals_triangles (new pcl::PointCloud<pcl::Normal>); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_triangles (new pcl::search::KdTree<pcl::PointXYZ>); tree_triangles->setInputCloud(planes_cloud); n.setInputCloud(planes_cloud); n.setSearchMethod(tree_triangles); n.setKSearch(20); n.compute(*normals_triangles); pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>); pcl::concatenateFields (*planes_cloud,*normals_triangles,*cloud_with_normals); pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>); tree2->setInputCloud(cloud_with_normals); pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3; pcl::PolygonMesh triangles; gp3.setSearchRadius(0.4);// Ursprünglich 0.025 gp3.setMu(5.0); // Ursprünglich 2.5 gp3.setMaximumNearestNeighbors(20); // davor 100 gp3.setMaximumSurfaceAngle(M_PI/4); // ursprünglich: M_PI/4 gp3.setMinimumAngle(M_PI/18); gp3.setMaximumAngle(2*M_PI/1); // ursprunglich: 2*M_PI/3 gp3.setNormalConsistency(false); gp3.setInputCloud(cloud_with_normals); gp3.setSearchMethod(tree2); gp3.reconstruct(triangles); std::vector<int> parts = gp3.getPartIDs(); std::vector<int> states = gp3.getPointStates(); // Oberfläche als STL-Modell abspeichern pcl::io::savePolygonFileSTL("Arena_24_7_2014.stl", triangles); //pcl::io::saveVTKFile("mesh.vtk", triangles); // Farbliche Visualisierung der segmentierten Punktwolke pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud (); pcl::visualization::CloudViewer viewer ("Cluster viewer"); viewer.showCloud(colored_cloud); viewer.runOnVisualizationThreadOnce(background_color); while (!viewer.wasStopped ()) { } /* pcl::PointCloud <pcl::PointXYZ>::Ptr cluster_cloud=clusters[0].indices; pcl::visualization::CloudViewer viewer ("Cluster viewer"); viewer.showCloud(cluster_cloud); while (!viewer.wasStopped()) { } */ return (0); }
void PwnTracker::processFrame(const DepthImage& depthImage_, const Eigen::Isometry3f& sensorOffset_, const Eigen::Matrix3f& cameraMatrix_, const Eigen::Isometry3f& initialGuess){ int r,c; Eigen::Matrix3f scaledCameraMatrix = cameraMatrix_; _currentCloudOffset = sensorOffset_; _currentCameraMatrix = cameraMatrix_; _currentDepthImage = depthImage_; _currentCloud = makeCloud(r,c,scaledCameraMatrix,_currentCloudOffset,_currentDepthImage); bool newFrame = false; bool newRelation = false; PwnTrackerFrame* currentTrackerFrame=0; Eigen::Isometry3d relationMean; if (_previousCloud){ _aligner->setCurrentSensorOffset(_currentCloudOffset); _aligner->setCurrentFrame(_currentCloud); _aligner->setReferenceSensorOffset(_previousCloudOffset); _aligner->setReferenceFrame(_previousCloud); _aligner->correspondenceFinder()->setImageSize(r,c); PinholePointProjector* alprojector=dynamic_cast<PinholePointProjector*>(_aligner->projector()); alprojector->setCameraMatrix(scaledCameraMatrix); alprojector->setImageSize(r,c); Eigen::Isometry3f guess=_previousCloudTransform.inverse()*_globalT*initialGuess; _aligner->setInitialGuess(guess); double t0, t1; t0 = g2o::get_time(); _aligner->align(); t1 = g2o::get_time(); std::cout << "Time: " << t1 - t0 << " seconds " << std::endl; 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()>0){ _globalT = _previousCloudTransform*_aligner->T(); //cerr << "TRANSFORM FOUND" << endl; } else { //cerr << "FAILURE" << endl; _globalT = _globalT*guess; } convertScalar(relationMean, _aligner->T()); 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; newAlignmentCallbacks(_globalT, _aligner->T(), _aligner->inliers(), _aligner->error()); int maxInliers = r*c; float inliersFraction = (float) _aligner->inliers()/(float) maxInliers; cerr << "inliers/maxinliers/fraction: " << _aligner->inliers() << "/" << maxInliers << "/" << inliersFraction << endl; if (inliersFraction<_newFrameInliersFraction){ newFrame = true; newRelation = true; // char filename[1024]; // sprintf (filename, "frame-%05d.pwn", _numKeyframes); // frame->save(filename,1,true,_globalT); _numKeyframes ++; if (!_cache) delete _previousCloud; else{ _previousCloudHandle.release(); } //_cache->unlock(_previousTrackerFrame); // _aligner->setReferenceSensorOffset(_currentCloudOffset); // _aligner->setReferenceFrame(_currentCloud); _previousCloud = _currentCloud; _previousCloudTransform = _globalT; // cerr << "new frame added (" << _numKeyframes << ")" << endl; // cerr << "inliers: " << _aligner->inliers() << endl; // cerr << "maxInliers: " << maxInliers << 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; // cerr << "globalTransform : " << t2v(_globalT).transpose() << endl; } else { // previous frame but offset is small delete _currentCloud; _currentCloud = 0; } } else { // first frame //ser.writeObject(*manager); newFrame = true; // _aligner->setReferenceSensorOffset(_currentCloudOffset); // _aligner->setReferenceFrame(_currentCloud); _previousCloud = _currentCloud; _previousCloudTransform = _globalT; _previousCloudOffset = _currentCloudOffset; _numKeyframes ++; /*Eigen::Isometry3f t = _globalT; geometry_msgs::Point p; p.x = t.translation().x(); p.y = t.translation().y(); p.z = t.translation().z(); m_odometry.points.push_back(p); */ } _counter++; if (newFrame) { //cerr << "maing new frame, previous: " << _previousTrackerFrame << endl; currentTrackerFrame = new PwnTrackerFrame(_manager); //currentTrackerFrame->cloud.set(cloud); currentTrackerFrame->cloud=_currentCloud; currentTrackerFrame->sensorOffset = _currentCloudOffset; boss_map::ImageBLOB* depthBLOB=new boss_map::ImageBLOB(); DepthImage_convert_32FC1_to_16UC1(depthBLOB->cvImage(),_currentDepthImage); //depthImage.toCvMat(depthBLOB->cvImage()); depthBLOB->adjustFormat(); currentTrackerFrame->depthImage.set(depthBLOB); boss_map::ImageBLOB* normalThumbnailBLOB=new boss_map::ImageBLOB(); boss_map::ImageBLOB* depthThumbnailBLOB=new boss_map::ImageBLOB(); makeThumbnails(depthThumbnailBLOB->cvImage(), normalThumbnailBLOB->cvImage(), _currentCloud, _currentDepthImage.rows, _currentDepthImage.cols, _currentCloudOffset, _currentCameraMatrix, 0.1); normalThumbnailBLOB->adjustFormat(); depthThumbnailBLOB->adjustFormat(); currentTrackerFrame->depthThumbnail.set(depthThumbnailBLOB); currentTrackerFrame->normalThumbnail.set(normalThumbnailBLOB); currentTrackerFrame->cameraMatrix = _currentCameraMatrix; currentTrackerFrame->imageRows = _currentDepthImage.rows; currentTrackerFrame->imageCols = _currentDepthImage.cols; Eigen::Isometry3d _iso; convertScalar(_iso, _globalT); currentTrackerFrame->setTransform(_iso); convertScalar(currentTrackerFrame->sensorOffset, _currentCloudOffset); currentTrackerFrame->setSeq(_seq++); _manager->addNode(currentTrackerFrame); //cerr << "AAAA" << endl; if (_cache) _currentCloudHandle=_cache->get(currentTrackerFrame); //cerr << "BBBB" << endl; //_cache->lock(currentTrackerFrame); newFrameCallbacks(currentTrackerFrame); currentTrackerFrame->depthThumbnail.set(0); currentTrackerFrame->normalThumbnail.set(0); currentTrackerFrame->depthImage.set(0); } if (newRelation) { PwnTrackerRelation* rel = new PwnTrackerRelation(_manager); rel->setTransform(relationMean); Matrix6d omega; convertScalar(omega, _aligner->omega()); omega.setIdentity(); omega *= 100; rel->setInformationMatrix(omega); rel->setTo(currentTrackerFrame); rel->setFrom(_previousTrackerFrame); //cerr << "saved relation" << _previousTrackerFrame << " " << currentTrackerFrame << endl; _manager->addRelation(rel); newRelationCallbacks(rel); //ser.writeObject(*rel); } if (currentTrackerFrame) { _previousTrackerFrame = currentTrackerFrame; } }
ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft, const Eigen::Vector3f& localAnkleRight, const Eigen::Matrix4f& leftFootPoseGroundFrame, const Eigen::Matrix4f& rightFootPoseGroundFrame, const Eigen::Vector3f& zmpRefGroundFrame, Bipedal::SupportPhase phase) { Eigen::Matrix4f groundPoseLeft = Bipedal::projectPoseToGround(leftFootPoseGroundFrame); Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame); Eigen::Vector3f localZMPLeft = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse()); Eigen::Vector3f localZMPRight = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse()); double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase); //std::cout << "########## " << alpha << " ###########" << std::endl; ForceTorque ft; // kg*m/s^2 = N ft.leftForce = -(1-alpha) * mass * gravity; ft.rightForce = -alpha * mass * gravity; // Note we need force as kg*mm/s^2 ft.leftTorque = (localAnkleLeft - localZMPLeft).cross(ft.leftForce * 1000); ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000); // ZMP not contained in convex polygone if (std::fabs(alpha) > std::numeric_limits<float>::epsilon() && std::fabs(alpha-1) > std::numeric_limits<float>::epsilon()) { Eigen::Vector3f leftTorqueGroundFrame = groundPoseLeft.block(0, 0, 3, 3) * ft.leftTorque; Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque; Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame); //std::cout << "Tau0World: " << tau0.transpose() << std::endl; //std::cout << "leftTorqueWorld: " << leftTorqueWorld.transpose() << std::endl; //std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl; // Note: Our coordinate system is different than in the paper! // Also this is not the same as the ground frame. Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft - localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1); xAxis /= xAxis.norm(); Eigen::Vector3f zAxis(0, 0, 1); Eigen::Vector3f yAxis = zAxis.cross(xAxis); yAxis /= yAxis.norm(); Eigen::Matrix3f centerFrame; centerFrame.block(0, 0, 3, 1) = xAxis; centerFrame.block(0, 1, 3, 1) = yAxis; centerFrame.block(0, 2, 3, 1) = zAxis; //std::cout << "Center frame:\n" << centerFrame << std::endl; Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0; Eigen::Vector3f leftTorqueCenter; leftTorqueCenter.x() = (1-alpha)*centerTau0.x(); leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0; leftTorqueCenter.z() = 0; Eigen::Vector3f rightTorqueCenter; rightTorqueCenter.x() = alpha*centerTau0.x(); rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y(); rightTorqueCenter.z() = 0; //std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl; //std::cout << "leftTorqueCenter: " << leftTorqueCenter.transpose() << std::endl; //std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl; // tcp <--- ground frame <--- center frame ft.leftTorque = groundPoseLeft.block(0, 0, 3, 3).transpose() * centerFrame * leftTorqueCenter; ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter; } // Torque depends on timestep, we need a way to do this correctly. const double torqueFactor = 1; // convert to Nm ft.leftTorque *= torqueFactor / 1000.0 / 1000.0; ft.rightTorque *= torqueFactor / 1000.0 / 1000.0; //std::cout << "leftTorque: " << ft.leftTorque.transpose() << std::endl; //std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl; return ft; }