bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation) { if (corres.size() >= 15) { vector<cv::Point2f> ll, rr; for (int i = 0; i < int(corres.size()); i++) { ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1))); rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1))); } cv::Mat mask; cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask); cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1); cv::Mat rot, trans; int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask); //cout << "inlier_cnt " << inlier_cnt << endl; Eigen::Matrix3d R; Eigen::Vector3d T; for (int i = 0; i < 3; i++) { T(i) = trans.at<double>(i, 0); for (int j = 0; j < 3; j++) R(i, j) = rot.at<double>(i, j); } Rotation = R.transpose(); Translation = -R.transpose() * T; if(inlier_cnt > 12) return true; else return false; } return false; }
double GreenStrain_LIMSolver3D::computeFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x) { // green strain energy double shape = 0; Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); for(int t=0;t<mesh->Tetrahedra->rows();t++) { Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]); Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]); Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]); Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]); Eigen::Matrix<double,3,4> V; V.col(0) = A; V.col(1) = B; V.col(2) = C; V.col(3) = D; Eigen::Matrix3d F = V*Ms.block<4,3>(0,3*t); Eigen::Matrix3d E = (F.transpose()*F - I); shape += E.squaredNorm()*Divider; } return shape; }
bool CGEOM::generate_scene_trans ( const SceneGeneratorOptions& sc_opts, Eigen::MatrixXd& mP3D, Eigen::MatrixXd& mMeasT, Eigen::MatrixXd& mMeasN, Eigen::Matrix3d& mR, Eigen::Vector3d& mt ) { if( !generate_scene( sc_opts, mP3D, mMeasT, mMeasN ) ) { return false; } // Create random transform mt = mP3D.rowwise().mean(); const double drotx = rand_range_d( -45., 45. )*3.14159/180.; const double droty = rand_range_d( -45., 45. )*3.14159/180.; const double drotz = rand_range_d( -45., 45. )*3.14159/180.; #if 1 mR = ( CEIGEN::skew_rot<Eigen::Matrix3d,double>( drotx, 0., 0. ) + CEIGEN::skew_rot<Eigen::Matrix3d,double>( 0., droty , 0.) + CEIGEN::skew_rot<Eigen::Matrix3d,double>( 0., 0., drotz ) ).exp(); #else mR = Eigen::Matrix3d::Identity(); #endif mP3D.colwise() -= mt; mP3D = mR.transpose() * mP3D; return true; }
// Assume t = double[3], q = double[4] void EstimateTfSVD(double* t, double* q) { // Assemble the correlation matrix H = target * reference' Eigen::Matrix3d H = (cloud_tgt_demean * cloud_ref_demean.transpose ()).topLeftCorner<3, 3>(); // Compute the Singular Value Decomposition Eigen::JacobiSVD<Eigen::Matrix3d> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d u = svd.matrixU (); Eigen::Matrix3d v = svd.matrixV (); // Compute R = V * U' if (u.determinant () * v.determinant () < 0) { for (int i = 0; i < 3; ++i) v (i, 2) *= -1; } // std::cout<< "centroid_src: "<<centroid_src(0) <<" "<< centroid_src(1) <<" "<< centroid_src(2) << " "<< centroid_src(3)<<std::endl; // std::cout<< "centroid_tgt: "<<centroid_tgt(0) <<" "<< centroid_tgt(1) <<" "<< centroid_tgt(2) << " "<< centroid_tgt(3)<<std::endl; Eigen::Matrix3d R = v * u.transpose (); const Eigen::Vector3d Rc (R * centroid_tgt.head<3> ()); Eigen::Vector3d T = centroid_ref.head<3> () - Rc; // Make sure these memory locations are valid assert(t != NULL && q!=NULL); Eigen::Quaterniond Q(R); t[0] = T(0); t[1] = T(1); t[2] = T(2); q[0] = Q.w(); q[1] = Q.x(); q[2] = Q.y(); q[3] = Q.z(); }
//============================================================================== // Note: Taken from Springer Handbook, chapter 2.2.11 void Inertia::computeSpatialTensor() { Eigen::Matrix3d C = math::makeSkewSymmetric(mCenterOfMass); // Top left mSpatialTensor.block<3,3>(0,0) = getMoment() + mMass*C*C.transpose(); // Bottom left mSpatialTensor.block<3,3>(3,0) = mMass*C.transpose(); // Top right mSpatialTensor.block<3,3>(0,3) = mMass*C; // Bottom right mSpatialTensor.block<3,3>(3,3) = mMass*Eigen::Matrix3d::Identity(); }
Eigen::Matrix3d LinearAlgebra::transposeMatrixM(const Eigen::Matrix3d& M) { Eigen::Matrix3d result; // TODO: return the transpose of matrix M result = M.transpose(); return result; }
//============================================================================== Eigen::Vector3d BallJoint::getPositionDifferencesStatic( const Eigen::Vector3d& _q2, const Eigen::Vector3d& _q1) const { const Eigen::Matrix3d R1 = convertToRotation(_q1); const Eigen::Matrix3d R2 = convertToRotation(_q2); return convertToPositions(R1.transpose() * R2); }
void fundamental2essential( Eigen::Matrix3d &Fundamental, Eigen::Matrix3d &kalibration, Eigen::Matrix3d &Essential) { Essential = kalibration.transpose()*(Fundamental*kalibration); Essential = Essential/Essential(2,2); return; }
const Eigen::Matrix3d CMiniVisionToolbox::getFundamental( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo, const Eigen::Matrix3d& p_matIntrinsic ) { //ds get essential matrix const Eigen::Matrix3d matEssential( CMiniVisionToolbox::getEssential( p_matTransformationFrom, p_matTransformationTo ) ); //ds compute fundamental matrix: http://en.wikipedia.org/wiki/Fundamental_matrix_%28computer_vision%29 const Eigen::Matrix3d matIntrinsicInverse( p_matIntrinsic.inverse( ) ); return matIntrinsicInverse.transpose( )*matEssential*matIntrinsicInverse; }
void pose_estimation_3d3d ( const vector<Point3f>& pts1, const vector<Point3f>& pts2, Mat& R, Mat& t ) { Point3f p1, p2; // center of mass int N = pts1.size(); for ( int i=0; i<N; i++ ) { p1 += pts1[i]; p2 += pts2[i]; } p1 = Point3f( Vec3f(p1) / N); p2 = Point3f( Vec3f(p2) / N); vector<Point3f> q1 ( N ), q2 ( N ); // remove the center for ( int i=0; i<N; i++ ) { q1[i] = pts1[i] - p1; q2[i] = pts2[i] - p2; } // compute q1*q2^T Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); for ( int i=0; i<N; i++ ) { W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose(); } cout<<"W="<<W<<endl; // SVD on W Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV ); Eigen::Matrix3d U = svd.matrixU(); Eigen::Matrix3d V = svd.matrixV(); if (U.determinant() * V.determinant() < 0) { for (int x = 0; x < 3; ++x) { U(x, 2) *= -1; } } cout<<"U="<<U<<endl; cout<<"V="<<V<<endl; Eigen::Matrix3d R_ = U* ( V.transpose() ); Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z ); // convert to cv::Mat R = ( Mat_<double> ( 3,3 ) << R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ), R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ), R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 ) ); t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) ); }
void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices) { type = "Oriented"; orientedPoints.clear(); // compute mean Eigen::Vector3d center; center.setZero(); for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) { center += v->position; } center /= (double)vertices.size(); // adjust for mean and compute covariance Eigen::Matrix3d covariance; covariance.setZero(); for (VertexIter v = vertices.begin(); v != vertices.end(); v++) { Eigen::Vector3d pAdg = v->position - center; covariance += pAdg * pAdg.transpose(); } covariance /= (double)vertices.size(); // compute eigenvectors for the covariance matrix Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance); Eigen::Matrix3d eigenVectors = solver.eigenvectors().real(); // project min and max points on each principal axis double min1 = INFINITY, max1 = -INFINITY; double min2 = INFINITY, max2 = -INFINITY; double min3 = INFINITY, max3 = -INFINITY; double d = 0.0; eigenVectors.transpose(); for (VertexIter v = vertices.begin(); v != vertices.end(); v++) { d = eigenVectors.row(0).dot(v->position); if (min1 > d) min1 = d; if (max1 < d) max1 = d; d = eigenVectors.row(1).dot(v->position); if (min2 > d) min2 = d; if (max2 < d) max2 = d; d = eigenVectors.row(2).dot(v->position); if (min3 > d) min3 = d; if (max3 < d) max3 = d; } // add points to vector orientedPoints.push_back(eigenVectors.row(0) * min1); orientedPoints.push_back(eigenVectors.row(0) * max1); orientedPoints.push_back(eigenVectors.row(1) * min2); orientedPoints.push_back(eigenVectors.row(1) * max2); orientedPoints.push_back(eigenVectors.row(2) * min3); orientedPoints.push_back(eigenVectors.row(2) * max3); }
ELASStereo::ELASStereo(calibu::CameraRig& rig, const unsigned int width, const unsigned int height) : m_dDisparity(width, height), m_dDepth(width, height) { if (rig.cameras.size() != 2) { std::cerr << "Two camera models are required to run this program!" << std::endl; exit(1); } m_width = width; m_height = height; Eigen::Matrix3f CamModel0 = rig.cameras[0].camera.K().cast<float>(); Eigen::Matrix3f CamModel1 = rig.cameras[1].camera.K().cast<float>(); roo::ImageIntrinsics camMod[] = { {CamModel0(0, 0), CamModel0(1, 1), CamModel0(0, 2), CamModel0(1, 2)}, {CamModel1(0, 0), CamModel1(1, 1), CamModel1(0, 2), CamModel1(1, 2)}}; m_Kl = camMod[0][0].Matrix(); // print selected camera model std::cout << "Camera Model used: " << std::endl << camMod[0][0].Matrix() << std::endl; Eigen::Matrix3d RDFvision; RDFvision << 1, 0, 0, 0, 1, 0, 0, 0, 1; Eigen::Matrix3d RDFrobot; RDFrobot << 0, 1, 0, 0, 0, 1, 1, 0, 0; Eigen::Matrix4d T_vis_ro = Eigen::Matrix4d::Identity(); T_vis_ro.block<3, 3>(0, 0) = RDFvision.transpose() * RDFrobot; Eigen::Matrix4d T_ro_vis = Eigen::Matrix4d::Identity(); T_ro_vis.block<3, 3>(0, 0) = RDFrobot.transpose() * RDFvision; const Sophus::SE3d T_rl = T_rlFromCamModelRDF(rig.cameras[0], rig.cameras[1], RDFvision); m_baseline = T_rl.translation().norm(); std::cout << "Baseline is: " << m_baseline << std::endl; }
void BoundingBox::computeOrientedBox(std::vector<Eigen::Vector3d>& positions) { // compute mean Eigen::Vector3d cm; cm.setZero(); for (size_t i = 0; i < positions.size(); i++) { cm += positions[i]; } cm /= (double)positions.size(); // adjust for mean and compute covariance matrix Eigen::Matrix3d covariance; covariance.setZero(); for (size_t i = 0; i < positions.size(); i++) { Eigen::Vector3d pAdg = positions[i] - cm; covariance += pAdg * pAdg.transpose(); } covariance /= (double)positions.size(); // compute eigenvectors for covariance matrix Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance); Eigen::Matrix3d eigenVectors = solver.eigenvectors().real(); // set axes eigenVectors.transpose(); xAxis = eigenVectors.row(0); yAxis = eigenVectors.row(1); zAxis = eigenVectors.row(2); // project min and max points on each principal axis double min1 = INF, max1 = -INF; double min2 = INF, max2 = -INF; double min3 = INF, max3 = -INF; double d = 0.0; for (size_t i = 0; i < positions.size(); i++) { d = xAxis.dot(positions[i]); if (min1 > d) min1 = d; if (max1 < d) max1 = d; d = yAxis.dot(positions[i]); if (min2 > d) min2 = d; if (max2 < d) max2 = d; d = zAxis.dot(positions[i]); if (min3 > d) min3 = d; if (max3 < d) max3 = d; } // set center and halflengths center = (xAxis*(min1 + max1) + yAxis*(min2 + max2) + zAxis*(min3 + max3)) /2; halfLx = (max1 - min1)/2; halfLy = (max2 - min2)/2; halfLz = (max3 - min3)/2; }
void poseJacobian(const Eigen::Matrix3d& rotation, Eigen::Matrix<double, 6, 6>& jac, const double rot_angle_threshold) { Eigen::Matrix3d i3 = Eigen::Matrix3d::Identity(); // convert rotation matrix into axis-angle representation double rot_angle; Eigen::Vector3d rot_axis; getAngleAxis(rotation.transpose(), rot_angle, rot_axis); // create the rotation jacobian Eigen::Matrix3d L_theta_u; double sinc_part; sinc_part = sva::sinc(rot_angle)/std::pow(sva::sinc(rot_angle/2.), 2); Eigen::Matrix3d axis_antisym; getSkewSym(rot_axis, axis_antisym); L_theta_u = i3 - rot_angle*0.5*axis_antisym + (1-(sinc_part))*axis_antisym*axis_antisym; jac << L_theta_u, Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), rotation.transpose(); }
IMPALGEBRA_BEGIN_NAMESPACE Eigen::Matrix3d get_covariance(const Gaussian3D &g) { Transformation3D trans = g.get_reference_frame().get_transformation_to(); Vector3D center = trans.get_translation(); Vector4D iq = trans.get_rotation().get_quaternion(); Eigen::Quaterniond q(iq[0], iq[1], iq[2], iq[3]); Eigen::Matrix3d rot = q.toRotationMatrix(); Vector3D variances = g.get_variances(); Eigen::Matrix3d rad = Eigen::Vector3d(variances[0], variances[1], variances[2]).asDiagonal(); Eigen::Matrix3d covar = rot * (rad * rot.transpose()); return covar; }
Eigen::Matrix3d Bd970::getPositionUncertainty(void) { Eigen::Matrix3d position_uncertainty; Eigen::Vector3d eigenvalues; // eigen values are the square of the std deviation eigenvalues << m_current_nmea.data_gst.semi_major_axis_sigma_error * m_current_nmea.data_gst.semi_major_axis_sigma_error, m_current_nmea.data_gst.semi_minor_axis_sigma_error * m_current_nmea.data_gst.semi_minor_axis_sigma_error, m_current_nmea.data_gst.height_sigma_error * m_current_nmea.data_gst.height_sigma_error; Eigen::Matrix3d U = this->getOrientation().matrix(); position_uncertainty = U * eigenvalues.array().matrix().asDiagonal() * U.transpose(); return position_uncertainty; }
void SFMViewer::update(std::vector<cv::Point3d> pcld, std::vector<cv::Vec3b> pcldrgb, std::vector<cv::Point3d> pcld_alternate, std::vector<cv::Vec3b> pcldrgb_alternate, std::vector<cv::Matx34d> cameras) { m_pcld = pcld; m_pcldrgb = pcldrgb; m_cameras = cameras; //get the scale of the result cloud using PCA { cv::Mat_<double> cldm(pcld.size(), 3); for (unsigned int i = 0; i < pcld.size(); i++) { cldm.row(i)(0) = pcld[i].x; cldm.row(i)(1) = pcld[i].y; cldm.row(i)(2) = pcld[i].z; } cv::Mat_<double> mean; //cv::reduce(cldm,mean,0,CV_REDUCE_AVG); cv::PCA pca(cldm, mean, CV_PCA_DATA_AS_ROW); scale_cameras_down = 1.0 / (3.0 * sqrt(pca.eigenvalues.at<double> (0))); // std::cout << "emean " << mean << std::endl; // m_global_transform = Eigen::Translation<double,3>(-Eigen::Map<Eigen::Vector3d>(mean[0])); } //compute transformation to place cameras in world m_cameras_transforms.resize(m_cameras.size()); Eigen::Vector3d c_sum(0,0,0); for (int i = 0; i < m_cameras.size(); ++i) { Eigen::Matrix<double, 3, 4> P = Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> >(m_cameras[i].val); Eigen::Matrix3d R = P.block(0, 0, 3, 3); Eigen::Vector3d t = P.block(0, 3, 3, 1); Eigen::Vector3d c = -R.transpose() * t; c_sum += c; m_cameras_transforms[i] = Eigen::Translation<double, 3>(c) * Eigen::Quaterniond(R) * Eigen::UniformScaling<double>(scale_cameras_down) ; } m_global_transform = Eigen::Translation<double,3>(-c_sum / (double)(m_cameras.size())); // m_global_transform = m_cameras_transforms[0].inverse(); updateGL(); }
Eigen::Matrix3d Reconstruction3D::computeEpipoleMat(const Eigen::Matrix3d& F) { Eigen::JacobiSVD<Eigen::MatrixXd> svd(F.transpose(), Eigen::ComputeFullV); Eigen::MatrixXd kernel = svd.matrixV().col(svd.matrixV().cols() - 1); Eigen::Matrix3d e(3, 3); e(0, 0) = kernel(0); e(0, 1) = kernel(1); e(0, 2) = kernel(2); e(1, 0) = kernel(3); e(1, 1) = kernel(4); e(1, 2) = kernel(5); e(2, 0) = kernel(6); e(2, 1) = kernel(7); e(2, 2) = kernel(8); return e; }
void Win3D::drag(int _x, int _y) { double deltaX = _x - mMouseX; double deltaY = _y - mMouseY; mMouseX = _x; mMouseY = _y; if (mRotate) { if (deltaX != 0 || deltaY != 0) mTrackBall.updateBall(_x, mWinHeight - _y); } if (mTranslate) { Eigen::Matrix3d rot = mTrackBall.getRotationMatrix(); mTrans += rot.transpose()*Eigen::Vector3d(deltaX, -deltaY, 0.0); } if (mZooming) { mZoom += deltaY*0.01; } glutPostRedisplay(); }
void Simulation :: dampVelocities( void ) { // [ Mueller - 2007 Sec: 3.5 ] for( unsigned mIdx = 0; mIdx < m_meshes.size(); ++mIdx ){ Vector x_cm; Vector v_cm; double sumMass = 0.; for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){ sumMass += v->mass; x_cm += v->position * v->mass; v_cm += v->velocity * v->mass; } x_cm /= sumMass; v_cm /= sumMass; Vector r; Vector L; Eigen::Matrix3d I = Eigen::Matrix3d::Zero(); for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){ r = v->position - x_cm; L += cross( r, v->mass * v->velocity ); Eigen::Matrix3d singleI = Eigen::Matrix3d::Zero(); singleI << 0., r[2], -r[1], -r[2], 0., r[0], r[2], -r[0], 0.; I += singleI * singleI.transpose() * v->mass; } Eigen::Vector3d L_tmp ( L[0], L[1], L[2] ); Eigen::Vector3d omegaTemp = I.inverse() * L_tmp; Vector ang_vel_omega( omegaTemp[0], omegaTemp[1], omegaTemp[2] ); for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){ r = v->position - x_cm; Vector delta_v = v_cm + cross( ang_vel_omega, r ) - v->velocity; v->velocity = v->velocity + ( m_meshes[mIdx]->dampingStiffness() * delta_v ); } } }
// Implementation from the T. Lee et al. paper // Control of complex maneuvers for a quadrotor UAV using geometric methods on SE(3) void LeePositionController::ComputeDesiredAngularAcc(const Eigen::Vector3d& acceleration, Eigen::Vector3d* angular_acceleration) const { assert(angular_acceleration); Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix(); // Get the desired rotation matrix. Eigen::Vector3d b1_des; double yaw = command_trajectory_.getYaw(); b1_des << cos(yaw), sin(yaw), 0; Eigen::Vector3d b3_des; b3_des = -acceleration / acceleration.norm(); Eigen::Vector3d b2_des; b2_des = b3_des.cross(b1_des); b2_des.normalize(); Eigen::Matrix3d R_des; R_des.col(0) = b2_des.cross(b3_des); R_des.col(1) = b2_des; R_des.col(2) = b3_des; // Angle error according to lee et al. Eigen::Matrix3d angle_error_matrix = 0.5 * (R_des.transpose() * R - R.transpose() * R_des); Eigen::Vector3d angle_error; vectorFromSkewMatrix(angle_error_matrix, &angle_error); // TODO(burrimi) include angular rate references at some point. Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero()); angular_rate_des[2] = command_trajectory_.getYawRate(); Eigen::Vector3d angular_rate_error = odometry_.angular_velocity - R_des.transpose() * R * angular_rate_des; *angular_acceleration = -1 * angle_error.cwiseProduct(normalized_attitude_gain_) - angular_rate_error.cwiseProduct(normalized_angular_rate_gain_) + odometry_.angular_velocity.cross(odometry_.angular_velocity); // we don't need the inertia matrix here }
template <typename PointSource, typename PointTarget> inline void pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) { pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal (); using namespace std; // Difference between consecutive transforms double delta = 0; // Get the size of the target const size_t N = indices_->size (); // Set the mahalanobis matrices to identity mahalanobis_.resize (N, Eigen::Matrix3d::Identity ()); // Compute target cloud covariance matrices if (target_covariances_.empty ()) computeCovariances<PointTarget> (target_, tree_, target_covariances_); // Compute input cloud covariance matrices if (input_covariances_.empty ()) computeCovariances<PointSource> (input_, tree_reciprocal_, input_covariances_); base_transformation_ = guess; nr_iterations_ = 0; converged_ = false; double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; std::vector<int> nn_indices (1); std::vector<float> nn_dists (1); while(!converged_) { size_t cnt = 0; std::vector<int> source_indices (indices_->size ()); std::vector<int> target_indices (indices_->size ()); // guess corresponds to base_t and transformation_ to t Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero (); for(size_t i = 0; i < 4; i++) for(size_t j = 0; j < 4; j++) for(size_t k = 0; k < 4; k++) transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j)); Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> (); for (size_t i = 0; i < N; i++) { PointSource query = output[i]; query.getVector4fMap () = guess * query.getVector4fMap (); query.getVector4fMap () = transformation_ * query.getVector4fMap (); if (!searchForNeighbors (query, nn_indices, nn_dists)) { PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]); return; } // Check if the distance to the nearest neighbor is smaller than the user imposed threshold if (nn_dists[0] < dist_threshold) { Eigen::Matrix3d &C1 = input_covariances_[i]; Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]]; Eigen::Matrix3d &M = mahalanobis_[i]; // M = R*C1 M = R * C1; // temp = M*R' + C2 = R*C1*R' + C2 Eigen::Matrix3d temp = M * R.transpose(); temp+= C2; // M = temp^-1 M = temp.inverse (); source_indices[cnt] = static_cast<int> (i); target_indices[cnt] = nn_indices[0]; cnt++; } } // Resize to the actual number of valid correspondences source_indices.resize(cnt); target_indices.resize(cnt); /* optimize transformation using the current assignment and Mahalanobis metrics*/ previous_transformation_ = transformation_; //optimization right here try { rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_); /* compute the delta from this iteration */ delta = 0.; for(int k = 0; k < 4; k++) { for(int l = 0; l < 4; l++) { double ratio = 1; if(k < 3 && l < 3) // rotation part of the transform ratio = 1./rotation_epsilon_; else ratio = 1./transformation_epsilon_; double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l)); if(c_delta > delta) delta = c_delta; } } } catch (PCLException &e) { PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ()); break; } nr_iterations_++; // Check for convergence if (nr_iterations_ >= max_iterations_ || delta < 1) { converged_ = true; previous_transformation_ = transformation_; PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ()); } else PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ()); } //for some reason the static equivalent methode raises an error // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0)); // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3); final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3); final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3); final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3); final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3); }
void derotate(const Eigen::Matrix3d& rot) { pos_ = rot.transpose() * pos_; dir_ = rot.transpose() * dir_; }
int main( int argc, char* argv[] ) { // Initialise window pangolin::View& container = SetupPangoGLWithCuda(1024, 768); size_t cu_mem_start, cu_mem_end, cu_mem_total; cudaMemGetInfo( &cu_mem_start, &cu_mem_total ); glClearColor(1,1,1,0); // Open video device hal::Camera video = OpenRpgCamera(argc,argv); // Capture first image pb::ImageArray images; // N cameras, each w*h in dimension, greyscale const size_t N = video.NumChannels(); if( N != 2 ) { std::cerr << "Two images are required to run this program!" << std::endl; exit(1); } const size_t nw = video.Width(); const size_t nh = video.Height(); // Capture first image video.Capture(images); // Downsample this image to process less pixels const int max_levels = 6; const int level = roo::GetLevelFromMaxPixels( nw, nh, 640*480 ); // const int level = 4; assert(level <= max_levels); // Find centered image crop which aligns to 16 pixels at given level const NppiRect roi = roo::GetCenteredAlignedRegion(nw,nh,16 << level,16 << level); // Load Camera intrinsics from file GetPot clArgs( argc, argv ); const std::string filename = clArgs.follow("","-cmod"); if( filename.empty() ) { std::cerr << "Camera models file is required!" << std::endl; exit(1); } const calibu::CameraRig rig = calibu::ReadXmlRig(filename); if( rig.cameras.size() != 2 ) { std::cerr << "Two camera models are required to run this program!" << std::endl; exit(1); } Eigen::Matrix3f CamModel0 = rig.cameras[0].camera.K().cast<float>(); Eigen::Matrix3f CamModel1 = rig.cameras[1].camera.K().cast<float>(); roo::ImageIntrinsics camMod[] = { {CamModel0(0,0),CamModel0(1,1),CamModel0(0,2),CamModel0(1,2)}, {CamModel1(0,0),CamModel1(1,1),CamModel1(0,2),CamModel1(1,2)} }; for(int i=0; i<2; ++i ) { // Adjust to match camera image dimensions const double scale = nw / rig.cameras[i].camera.Width(); roo::ImageIntrinsics camModel = camMod[i].Scale( scale ); // Adjust to match cropped aligned image camModel = camModel.CropToROI( roi ); camMod[i] = camModel; } const unsigned int w = roi.width; const unsigned int h = roi.height; const unsigned int lw = w >> level; const unsigned int lh = h >> level; const Eigen::Matrix3d& K0 = camMod[0].Matrix(); const Eigen::Matrix3d& Kl = camMod[0][level].Matrix(); std::cout << "K Matrix: " << std::endl << K0 << std::endl; std::cout << "K Matrix - Level: " << std::endl << Kl << std::endl; std::cout << "Video stream dimensions: " << nw << "x" << nh << std::endl; std::cout << "Chosen Level: " << level << std::endl; std::cout << "Processing dimensions: " << lw << "x" << lh << std::endl; std::cout << "Offset: " << roi.x << "x" << roi.y << std::endl; // print selected camera model std::cout << "Camera Model used: " << std::endl << camMod[0][level].Matrix() << std::endl; Eigen::Matrix3d RDFvision;RDFvision<< 1,0,0, 0,1,0, 0,0,1; Eigen::Matrix3d RDFrobot; RDFrobot << 0,1,0, 0,0, 1, 1,0,0; Eigen::Matrix4d T_vis_ro = Eigen::Matrix4d::Identity(); T_vis_ro.block<3,3>(0,0) = RDFvision.transpose() * RDFrobot; Eigen::Matrix4d T_ro_vis = Eigen::Matrix4d::Identity(); T_ro_vis.block<3,3>(0,0) = RDFrobot.transpose() * RDFvision; const Sophus::SE3d T_rl_orig = T_rlFromCamModelRDF(rig.cameras[0], rig.cameras[1], RDFvision); // TODO(jmf): For now, assume cameras are rectified. Later allow unrectified cameras. /* double k1 = 0; double k2 = 0; if(cam[0].Type() == MVL_CAMERA_WARPED) { k1 = cam[0].GetModel()->warped.kappa1; k2 = cam[0].GetModel()->warped.kappa2; } */ const bool rectify = false; if(!rectify) { std::cout << "Using pre-rectified images" << std::endl; } // Check we received at least two images if(images.Size() < 2) { std::cerr << "Failed to capture first stereo pair from camera" << std::endl; return -1; } // Define Camera Render Object (for view / scene browsing) pangolin::OpenGlRenderState s_cam( pangolin::ProjectionMatrixRDF_TopLeft(w,h,K0(0,0),K0(1,1),K0(0,2),K0(1,2),0.1,10000), pangolin::IdentityMatrix(pangolin::GlModelViewStack) ); pangolin::GlBufferCudaPtr vbo(pangolin::GlArrayBuffer, lw*lh,GL_FLOAT, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW ); pangolin::GlBufferCudaPtr cbo(pangolin::GlArrayBuffer, lw*lh,GL_UNSIGNED_BYTE, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW ); pangolin::GlBuffer ibo = pangolin::MakeTriangleStripIboForVbo(lw,lh); // Allocate Camera Images on device for processing roo::Image<unsigned char, roo::TargetHost, roo::DontManage> hCamImg[] = {{0,nw,nh},{0,nw,nh}}; roo::Image<float2, roo::TargetDevice, roo::Manage> dLookup[] = {{w,h},{w,h}}; roo::Image<unsigned char, roo::TargetDevice, roo::Manage> upload(w,h); roo::Pyramid<unsigned char, max_levels, roo::TargetDevice, roo::Manage> img_pyr[] = {{w,h},{w,h}}; roo::Image<float, roo::TargetDevice, roo::Manage> img[] = {{lw,lh},{lw,lh}}; roo::Volume<float, roo::TargetDevice, roo::Manage> vol[] = {{lw,lh,MAXD},{lw,lh,MAXD}}; roo::Image<float, roo::TargetDevice, roo::Manage> disp[] = {{lw,lh},{lw,lh}}; roo::Image<float, roo::TargetDevice, roo::Manage> meanI(lw,lh); roo::Image<float, roo::TargetDevice, roo::Manage> varI(lw,lh); roo::Image<float, roo::TargetDevice, roo::Manage> temp[] = {{lw,lh},{lw,lh},{lw,lh},{lw,lh},{lw,lh}}; roo::Image<float,roo::TargetDevice, roo::Manage>& imgd = disp[0]; roo::Image<float,roo::TargetDevice, roo::Manage> depthmap(lw,lh); roo::Image<float,roo::TargetDevice, roo::Manage> imga(lw,lh); roo::Image<float2,roo::TargetDevice, roo::Manage> imgq(lw,lh); roo::Image<float,roo::TargetDevice, roo::Manage> imgw(lw,lh); roo::Image<float4, roo::TargetDevice, roo::Manage> d3d(lw,lh); roo::Image<unsigned char, roo::TargetDevice,roo::Manage> Scratch(lw*sizeof(roo::LeastSquaresSystem<float,6>),lh); typedef ulong4 census_t; roo::Image<census_t, roo::TargetDevice, roo::Manage> census[] = {{lw,lh},{lw,lh}}; // Stereo transformation (post-rectification) Sophus::SE3d T_rl = T_rl_orig; const double baseline = T_rl.translation().norm(); std::cout << "Baseline: " << baseline << std::endl; cudaMemGetInfo( &cu_mem_end, &cu_mem_total ); std::cout << "CuTotal: " << cu_mem_total/(1024*1024) << ", Available: " << cu_mem_end/(1024*1024) << ", Used: " << (cu_mem_start-cu_mem_end)/(1024*1024) << std::endl; pangolin::Var<bool> step("ui.step", false, false); pangolin::Var<bool> run("ui.run", false, true); pangolin::Var<bool> lockToCam("ui.Lock to cam", false, true); pangolin::Var<int> show_slice("ui.show slice",MAXD/2, 0, MAXD-1); pangolin::Var<int> maxdisp("ui.maxdisp",MAXD, 0, MAXD); pangolin::Var<bool> subpix("ui.subpix", true, true); pangolin::Var<bool> use_census("ui.use census", true, true); pangolin::Var<int> avg_rad("ui.avg_rad",0, 0, 100); pangolin::Var<bool> do_dtam("ui.do dtam", false, true); pangolin::Var<bool> dtam_reset("ui.reset", false, false); pangolin::Var<float> g_alpha("ui.g alpha", 14, 0,4); pangolin::Var<float> g_beta("ui.g beta", 2.5, 0,2); pangolin::Var<float> theta("ui.theta", 100, 0,100); pangolin::Var<float> lambda("ui.lambda", 20, 0,20); pangolin::Var<float> sigma_q("ui.sigma q", 0.7, 0, 1); pangolin::Var<float> sigma_d("ui.sigma d", 0.7, 0, 1); pangolin::Var<float> huber_alpha("ui.huber alpha", 0.002, 0, 0.01); pangolin::Var<float> beta("ui.beta", 0.00001, 0, 0.01); pangolin::Var<float> alpha("ui.alpha", 0.9, 0,1); pangolin::Var<float> r1("ui.r1", 100, 0,0.01); pangolin::Var<float> r2("ui.r2", 100, 0,0.01); pangolin::Var<bool> filter("ui.filter", false, true); pangolin::Var<float> eps("ui.eps",0.01*0.01, 0, 0.01); pangolin::Var<int> rad("ui.radius",9, 1, 20); pangolin::Var<bool> leftrightcheck("ui.left-right check", false, true); pangolin::Var<float> maxdispdiff("ui.maxdispdiff",1, 0, 5); pangolin::Var<int> domedits("ui.median its",1, 1, 10); pangolin::Var<bool> domed9x9("ui.median 9x9", false, true); pangolin::Var<bool> domed7x7("ui.median 7x7", false, true); pangolin::Var<bool> domed5x5("ui.median 5x5", false, true); pangolin::Var<int> medi("ui.medi",12, 0, 24); pangolin::Var<float> filtgradthresh("ui.filt grad thresh", 0, 0, 20); pangolin::Var<bool> save_depthmaps("ui.save_depthmaps", false, true); int jump_frames = 0; pangolin::RegisterKeyPressCallback(' ', [&run](){run = !run;} ); pangolin::RegisterKeyPressCallback('l', [&lockToCam](){lockToCam = !lockToCam;} ); pangolin::RegisterKeyPressCallback(pangolin::PANGO_SPECIAL + GLUT_KEY_RIGHT, [&step](){step=true;} ); pangolin::RegisterKeyPressCallback(']', [&jump_frames](){jump_frames=100;} ); pangolin::RegisterKeyPressCallback('}', [&jump_frames](){jump_frames=1000;} ); pangolin::Handler2dImageSelect handler2d(lw,lh,level); // ActivateDrawPyramid<unsigned char,max_levels> adleft(img_pyr[0],GL_LUMINANCE8, false, true); // ActivateDrawPyramid<unsigned char,max_levels> adright(img_pyr[1],GL_LUMINANCE8, false, true); pangolin::ActivateDrawImage<float> adleft(img[0],GL_LUMINANCE32F_ARB, false, true); pangolin::ActivateDrawImage<float> adright(img[1],GL_LUMINANCE32F_ARB, false, true); pangolin::ActivateDrawImage<float> adisp(disp[0],GL_LUMINANCE32F_ARB, false, true); pangolin::ActivateDrawImage<float> adw(imgw,GL_LUMINANCE32F_ARB, false, true); // ActivateDrawImage<float> adCrossSection(dCrossSection,GL_RGBA_FLOAT32_APPLE, false, true); pangolin::ActivateDrawImage<float> adVol(vol[0].ImageXY(show_slice),GL_LUMINANCE32F_ARB, false, true); SceneGraph::GLSceneGraph graph; SceneGraph::GLVbo glvbo(&vbo,&ibo,&cbo); graph.AddChild(&glvbo); SetupContainer(container, 6, (float)w/h); container[0].SetDrawFunction(boost::ref(adleft)).SetHandler(&handler2d); container[1].SetDrawFunction(boost::ref(adright)).SetHandler(&handler2d); container[2].SetDrawFunction(boost::ref(adisp)).SetHandler(&handler2d); container[3].SetDrawFunction(boost::ref(adVol)).SetHandler(&handler2d); container[4].SetDrawFunction(SceneGraph::ActivateDrawFunctor(graph, s_cam)) .SetHandler( new pangolin::Handler3D(s_cam, pangolin::AxisNone) ); container[5].SetDrawFunction(boost::ref(adw)).SetHandler(&handler2d); for(unsigned long frame=0; !pangolin::ShouldQuit();) { bool go = frame==0 || jump_frames > 0 || run || Pushed(step); for(; jump_frames > 0; jump_frames--) { video.Capture(images); } if(go) { if(frame>0) { if( video.Capture(images) == false) { exit(1); } } frame++; ///////////////////////////////////////////////////////////// // Upload images to device (Warp / Decimate if necessery) for(int i=0; i<2; ++i ) { hCamImg[i].ptr = images[i].data(); if(rectify) { upload.CopyFrom(hCamImg[i].SubImage(roi)); Warp(img_pyr[i][0], upload, dLookup[i]); }else{ img_pyr[i][0].CopyFrom(hCamImg[i].SubImage(roi)); } roo::BoxReduce<unsigned char, max_levels, unsigned int>(img_pyr[i]); } } go |= avg_rad.GuiChanged() | use_census.GuiChanged(); if( go ) { for(int i=0; i<2; ++i ) { roo::ElementwiseScaleBias<float,unsigned char,float>(img[i], img_pyr[i][level],1.0f/255.0f); if(avg_rad > 0 ) { roo::BoxFilter<float,float,float>(temp[0],img[i],Scratch,avg_rad); roo::ElementwiseAdd<float,float,float,float>(img[i], img[i], temp[0], 1, -1, 0.5); } if(use_census) { Census(census[i], img[i]); } } } if( go | g_alpha.GuiChanged() || g_beta.GuiChanged() ) { ExponentialEdgeWeight(imgw, img[0], g_alpha, g_beta); } go |= filter.GuiChanged() | leftrightcheck.GuiChanged() | rad.GuiChanged() | eps.GuiChanged() | alpha.GuiChanged() | r1.GuiChanged() | r2.GuiChanged(); if(go) { if(use_census) { roo::CensusStereoVolume<float, census_t>(vol[0], census[0], census[1], maxdisp, -1); if(leftrightcheck) roo::CensusStereoVolume<float, census_t>(vol[1], census[1], census[0], maxdisp, +1); }else{ CostVolumeFromStereoTruncatedAbsAndGrad(vol[0], img[0], img[1], -1, alpha, r1, r2); if(leftrightcheck) CostVolumeFromStereoTruncatedAbsAndGrad(vol[1], img[1], img[0], +1, alpha, r1, r2); } if(filter) { // Filter Cost volume for(int v=0; v<(leftrightcheck?2:1); ++v) { roo::Image<float, roo::TargetDevice, roo::Manage>& I = img[v]; roo::ComputeMeanVarience<float,float,float>(varI, temp[0], meanI, I, Scratch, rad); for(int d=0; d<maxdisp; ++d) { roo::Image<float> P = vol[v].ImageXY(d); roo::ComputeCovariance(temp[0],temp[2],temp[1],P,meanI,I,Scratch,rad); GuidedFilter(P,temp[0],varI,temp[1],meanI,I,Scratch,temp[2],temp[3],temp[4],rad,eps); } } } } static int n = 0; // static float theta = 0; // go |= Pushed(dtam_reset); // if(go ) if(Pushed(dtam_reset)) { n = 0; theta.Reset(); // Initialise primal and auxillary variables CostVolMinimumSubpix(imgd,vol[0], maxdisp,-1); imga.CopyFrom(imgd); // Initialise dual variable imgq.Memset(0); } const double min_theta = 1E-0; if(do_dtam && theta > min_theta) { for(int i=0; i<5; ++i ) { // Auxillary exhaustive search CostVolMinimumSquarePenaltySubpix(imga, vol[0], imgd, maxdisp, -1, lambda, (theta) ); // Dual Ascent roo::WeightedHuberGradU_DualAscentP(imgq, imgd, imgw, sigma_q, huber_alpha); // Primal Descent roo::WeightedL2_u_minus_g_PrimalDescent(imgd, imgq, imga, imgw, sigma_d, 1.0f / (theta) ); theta= theta * (1-beta*n); ++n; } if( theta <= min_theta && save_depthmaps ) { cv::Mat dmap = cv::Mat( lh, lw, CV_32FC1 ); // convert disparity to depth roo::Disp2Depth(imgd, depthmap, Kl(0,0), baseline ); depthmap.MemcpyToHost( dmap.data ); // save depth image char Index[10]; sprintf( Index, "%05d", frame ); std::string DepthPrefix = "SDepth-"; std::string DepthFile; DepthFile = DepthPrefix + Index + ".pdm"; std::cout << "Depth File: " << DepthFile << std::endl; std::ofstream pDFile( DepthFile.c_str(), std::ios::out | std::ios::binary ); pDFile << "P7" << std::endl; pDFile << dmap.cols << " " << dmap.rows << std::endl; unsigned int Size = dmap.elemSize1() * dmap.rows * dmap.cols; pDFile << 4294967295 << std::endl; pDFile.write( (const char*)dmap.data, Size ); pDFile.close(); // save grey image std::string GreyPrefix = "Left-"; std::string GreyFile; GreyFile = GreyPrefix + Index + ".pgm"; std::cout << "Grey File: " << GreyFile << std::endl; cv::Mat gimg = cv::Mat( lh, lw, CV_8UC1 ); img_pyr[0][level].MemcpyToHost( gimg.data ); cv::imwrite( GreyFile, gimg ); // reset step = true; dtam_reset = true; } } go |= pangolin::GuiVarHasChanged(); // if(go) { // if(subpix) { // CostVolMinimumSubpix(disp[0],vol[0], maxdisp,-1); // if(leftrightcheck) CostVolMinimumSubpix(disp[1],vol[1], maxdisp,+1); // }else{ // CostVolMinimum<float,float>(disp[0],vol[0], maxdisp); // if(leftrightcheck) CostVolMinimum<float,float>(disp[1],vol[1], maxdisp); // } // } if(go) { for(int di=0; di<(leftrightcheck?2:1); ++di) { for(int i=0; i < domedits; ++i ) { if(domed9x9) MedianFilterRejectNegative9x9(disp[di],disp[di], medi); if(domed7x7) MedianFilterRejectNegative7x7(disp[di],disp[di], medi); if(domed5x5) MedianFilterRejectNegative5x5(disp[di],disp[di], medi); } } if(leftrightcheck ) { LeftRightCheck(disp[1], disp[0], +1, maxdispdiff); LeftRightCheck(disp[0], disp[1], -1, maxdispdiff); } if(filtgradthresh > 0) { FilterDispGrad(disp[0], disp[0], filtgradthresh); } } // if(go) { // Generate point cloud from disparity image DisparityImageToVbo(d3d, disp[0], baseline, Kl(0,0), Kl(1,1), Kl(0,2), Kl(1,2) ); // if(container[3].IsShown()) { // Copy point cloud into VBO { pangolin::CudaScopedMappedPtr var(vbo); roo::Image<float4> dVbo((float4*)*var,lw,lh); dVbo.CopyFrom(d3d); } // Generate CBO { pangolin::CudaScopedMappedPtr var(cbo); roo::Image<uchar4> dCbo((uchar4*)*var,lw,lh); roo::ConvertImage<uchar4,unsigned char>(dCbo, img_pyr[0][level]); } } // Update texture views adisp.SetImageScale(1.0f/maxdisp); // adleft.SetLevel(show_level); // adright.SetLevel(show_level); adVol.SetImage(vol[0].ImageXY(show_slice)); } ///////////////////////////////////////////////////////////// // Draw glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glColor3f(1,1,1); pangolin::FinishFrame(); } }
void ShapeMatching<PFP>::shapeMatch() { // p_{i} std::vector<Eigen::Vector3d> m_p; m_p.reserve(m_position.nbElements()); //1. Eigen::Vector3d xcm = massCenter(); for(unsigned int i = m_position.begin() ; i < m_position.end() ; m_position.next(i)) { Eigen::Vector3d tmp ; for (unsigned int j = 0 ; j < 3 ; ++j) tmp(j) = m_position[i][j] ; Eigen::Vector3d pi = tmp - xcm ; m_p.push_back(pi) ; //p_{i} = x_{i} - x_{cm} } //2. Eigen::Matrix3d apq = Eigen::Matrix3d::Zero(); for(unsigned int i=0 ; i < m_p.size() ; ++i) { apq(0,0) += m_p[i][0] * m_q[i][0]; apq(0,1) += m_p[i][0] * m_q[i][1]; apq(0,2) += m_p[i][0] * m_q[i][2]; apq(1,0) += m_p[i][1] * m_q[i][0]; apq(1,1) += m_p[i][1] * m_q[i][1]; apq(1,2) += m_p[i][1] * m_q[i][2]; apq(2,0) += m_p[i][2] * m_q[i][0]; apq(2,1) += m_p[i][2] * m_q[i][1]; apq(2,2) += m_p[i][2] * m_q[i][2]; } Eigen::Matrix3d S = apq.transpose() * apq ; //symmetric matrix //3. Jacobi Diagonalisation Eigen::EigenSolver<Eigen::Matrix3d> es(S); //V * D * V^(-1) Eigen::Matrix3d D = es.pseudoEigenvalueMatrix(); Eigen::Matrix3d U = es.pseudoEigenvectors() ; for(int j = 0; j < 3; j++) { if(D(j,j) <= 0) { D(j,j) = 0.05f; } D(j,j) = 1.0f/sqrt(D(j,j)); } S = U * D * U.transpose(); // Now we can get the rotation part Eigen::Matrix3d R = apq * S; //S^{-1} //4. for(unsigned int i = m_goal.begin() ; i < m_goal.end() ; m_goal.next(i)) { Eigen::Vector3d tmp = R * m_q[i] + xcm; // g_{i} = R * q_i + x_{cm} VEC3 g; for (unsigned int j = 0 ; j < 3 ; ++j) g[j] = tmp(j); m_goal[i] = g; } }
MatrixXd Tetrahedra<ConcreteShape>::buildStiffnessMatrix(const Ref<const VectorXd>& vp2) { Eigen::Matrix3d invJ; double detJ; std::tie(invJ,detJ) = ConcreteShape::inverseJacobian(mVtxCrd); // mGradientPhi_dr_t = mGradientPhi_dr.transpose(); // mGradientPhi_ds_t = mGradientPhi_ds.transpose(); // mGradientPhi_dt_t = mGradientPhi_dt.transpose(); // save for later mInvJac = invJ; mInvJacT_x_invJac = invJ.transpose() * invJ; mInvJacT = invJ.transpose(); mDetJac = detJ; //Jinv= rx, sx, tx, // ry, sy, ty, // rz, sz, tz; auto drdx = invJ(0,0); auto dsdx = invJ(0,1); auto dtdx = invJ(0,2); auto drdy = invJ(1,0); auto dsdy = invJ(1,1); auto dtdy = invJ(1,2); auto drdz = invJ(2,0); auto dsdz = invJ(2,1); auto dtdz = invJ(2,2); // build material on all nodes MatrixXd elementStiffnessMatrix(mNumIntPnt,mNumIntPnt); mGradientPhi_dx.resize(mNumIntPnt,mNumIntPnt); mGradientPhi_dy.resize(mNumIntPnt,mNumIntPnt); mGradientPhi_dz.resize(mNumIntPnt,mNumIntPnt); mWiDPhi_x.resize(mNumIntPnt,mNumIntPnt); mWiDPhi_y.resize(mNumIntPnt,mNumIntPnt); mWiDPhi_z.resize(mNumIntPnt,mNumIntPnt); // Stiffness Matrix // Dx = rx*Dr + sx*Ds + tx*Dt; // Dy = ry*Dr + sy*Ds + ty*Dt; // Dz = rz*Dr + sz*Ds + tz*Dt; // Kxx = detJ*Dx'*Me*Dx // Kyy = detJ*Dy'*Me*Dy // Kzz = detJ*Dz'*Me*Dz // K = Kxx+Kyy+Kzz // alternate method mGradientPhi_dx = (drdx*mGradientPhi_dr + dsdx*mGradientPhi_ds + dtdx*mGradientPhi_dt).transpose(); mGradientPhi_dy = (drdy*mGradientPhi_dr + dsdy*mGradientPhi_ds + dtdy*mGradientPhi_dt).transpose(); mGradientPhi_dz = (drdz*mGradientPhi_dr + dsdz*mGradientPhi_ds + dtdz*mGradientPhi_dt).transpose(); // Me*Dx, Me*Dy, Me*Dz RealVec wi_vp2 = mIntegrationWeights * vp2; mWiDPhi_x = (wi_vp2).asDiagonal() * mGradientPhi_dx; mWiDPhi_y = (wi_vp2).asDiagonal() * mGradientPhi_dy; mWiDPhi_z = (wi_vp2).asDiagonal() * mGradientPhi_dz; // for(int i=0;i<mNumIntPnt;i++) { // mWiDPhi_x.row(i) = mGradientPhi_dx.row(i)*mIntegrationWeights[i]; // mWiDPhi_y.row(i) = mGradientPhi_dy.row(i)*mIntegrationWeights[i]; // mWiDPhi_z.row(i) = mGradientPhi_dz.row(i)*mIntegrationWeights[i]; // } elementStiffnessMatrix = (detJ*mGradientPhi_dx.transpose()*mWiDPhi_x) + (detJ*mGradientPhi_dy.transpose()*mWiDPhi_y) + (detJ*mGradientPhi_dz.transpose()*mWiDPhi_z); // build transpose as well // mGradientPhi_dx_t = mGradientPhi_dx.transpose(); // mGradientPhi_dy_t = mGradientPhi_dy.transpose(); // mGradientPhi_dz_t = mGradientPhi_dz.transpose(); return elementStiffnessMatrix; }
//DEPRECATED??? double NDTMatcherFeatureD2D::scoreNDT(std::vector<NDTCell*> &sourceNDT, NDTMap &targetNDT, Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T) { NUMBER_OF_ACTIVE_CELLS = 0; double score_here = 0; double det = 0; bool exists = false; NDTCell *cell; Eigen::Matrix3d covCombined, icov; Eigen::Vector3d meanFixed; Eigen::Vector3d meanMoving; Eigen::Matrix3d R = T.rotation(); std::vector<std::pair<unsigned int, double> > scores; for(unsigned int j=0; j<_corr.size(); j++) { unsigned int i = _corr[j].second; if (_corr[j].second >= (int)sourceNDT.size()) { std::cout << "second correspondance : " << _corr[j].second << ", " << sourceNDT.size() << std::endl; } if (sourceNDT[i] == NULL) { std::cout << "sourceNDT[i] == NULL!" << std::endl; } meanMoving = T*sourceNDT[i]->getMean(); cell = targetNDT.getCellIdx(_corr[j].first); { if(cell == NULL) { std::cout << "cell== NULL!!!" << std::endl; } else { if(cell->hasGaussian_) { meanFixed = cell->getMean(); covCombined = cell->getCov() + R.transpose()*sourceNDT[i]->getCov()*R; covCombined.computeInverseAndDetWithCheck(icov,det,exists); if(!exists) continue; double l = (meanMoving-meanFixed).dot(icov*(meanMoving-meanFixed)); if(l*0 != 0) continue; if(l > 120) continue; double sh = -lfd1*(exp(-lfd2*l/2)); if(fabsf(sh) > 1e-10) { NUMBER_OF_ACTIVE_CELLS++; } scores.push_back(std::pair<unsigned int, double>(j, sh)); score_here += sh; //score_here += l; } } } } if (_trimFactor == 1.) { return score_here; } else { // Determine the score value if (scores.empty()) // TODO, this happens(!), why??!?? return score_here; score_here = 0.; unsigned int index = static_cast<unsigned int>(_trimFactor * (scores.size() - 1)); // std::nth_element (scores.begin(), scores.begin()+index, scores.end(), sort_scores()); //boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2)); std::nth_element (scores.begin(), scores.begin()+index, scores.end(), boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2)); std::fill(_goodCorr.begin(), _goodCorr.end(), false); // std::cout << "_goodCorr.size() : " << _goodCorr.size() << " scores.size() : " << scores.size() << " index : " << index << std::endl; for (unsigned int i = 0; i < _goodCorr.size(); i++) { if (i <= index) { score_here += scores[i].second; _goodCorr[scores[i].first] = true; } } return score_here; } }
int P3P::computePoses(const Eigen::Matrix3d & feature_vectors, const Eigen::Matrix3d & world_points, Eigen::Matrix<Eigen::Matrix<double, 3, 4>, 4, 1> & solutions) { // Extraction of world points Eigen::Vector3d P1 = world_points.col(0); Eigen::Vector3d P2 = world_points.col(1); Eigen::Vector3d P3 = world_points.col(2); // Verification that world points are not colinear Eigen::Vector3d temp1 = P2 - P1; Eigen::Vector3d temp2 = P3 - P1; if (temp1.cross(temp2).norm() == 0) { return -1; } // Extraction of feature vectors Eigen::Vector3d f1 = feature_vectors.col(0); Eigen::Vector3d f2 = feature_vectors.col(1); Eigen::Vector3d f3 = feature_vectors.col(2); // Creation of intermediate camera frame Eigen::Vector3d e1 = f1; Eigen::Vector3d e3 = f1.cross(f2); e3 = e3 / e3.norm(); Eigen::Vector3d e2 = e3.cross(e1); Eigen::Matrix3d T; T.row(0) = e1.transpose(); T.row(1) = e2.transpose(); T.row(2) = e3.transpose(); f3 = T * f3; // Reinforce that f3(2,0) > 0 for having theta in [0;pi] if (f3(2, 0) > 0) { f1 = feature_vectors.col(1); f2 = feature_vectors.col(0); f3 = feature_vectors.col(2); e1 = f1; e3 = f1.cross(f2); e3 = e3 / e3.norm(); e2 = e3.cross(e1); T.row(0) = e1.transpose(); T.row(1) = e2.transpose(); T.row(2) = e3.transpose(); f3 = T * f3; P1 = world_points.col(1); P2 = world_points.col(0); P3 = world_points.col(2); } // Creation of intermediate world frame Eigen::Vector3d n1 = P2 - P1; n1 = n1 / n1.norm(); Eigen::Vector3d n3 = n1.cross(P3 - P1); n3 = n3 / n3.norm(); Eigen::Vector3d n2 = n3.cross(n1); Eigen::Matrix3d N; N.row(0) = n1.transpose(); N.row(1) = n2.transpose(); N.row(2) = n3.transpose(); // Extraction of known parameters P3 = N * (P3 - P1); double d_12 = (P2 - P1).norm(); double f_1 = f3(0, 0) / f3(2, 0); double f_2 = f3(1, 0) / f3(2, 0); double p_1 = P3(0, 0); double p_2 = P3(1, 0); double cos_beta = f1.dot(f2); double b = 1 / (1 - pow(cos_beta, 2)) - 1; if (cos_beta < 0) { b = -sqrt(b); } else { b = sqrt(b); } // Definition of temporary variables for avoiding multiple computation double f_1_pw2 = pow(f_1, 2); double f_2_pw2 = pow(f_2, 2); double p_1_pw2 = pow(p_1, 2); double p_1_pw3 = p_1_pw2 * p_1; double p_1_pw4 = p_1_pw3 * p_1; double p_2_pw2 = pow(p_2, 2); double p_2_pw3 = p_2_pw2 * p_2; double p_2_pw4 = p_2_pw3 * p_2; double d_12_pw2 = pow(d_12, 2); double b_pw2 = pow(b, 2); // Computation of factors of 4th degree polynomial Eigen::Matrix<double, 5, 1> factors; factors(0) = -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4; factors(1) = 2 * p_2_pw3 * d_12 * b + 2 * f_2_pw2 * p_2_pw3 * d_12 * b - 2 * f_2 * p_2_pw3 * f_1 * d_12; factors(2) = -f_2_pw2 * p_2_pw2 * p_1_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw4 + p_2_pw4 * f_1_pw2 + 2 * p_1 * p_2_pw2 * d_12 + 2 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * b - p_2_pw2 * p_1_pw2 * f_1_pw2 + 2 * p_1 * p_2_pw2 * f_2_pw2 * d_12 - p_2_pw2 * d_12_pw2 * b_pw2 - 2 * p_1_pw2 * p_2_pw2; factors(3) = 2 * p_1_pw2 * p_2 * d_12 * b + 2 * f_2 * p_2_pw3 * f_1 * d_12 - 2 * f_2_pw2 * p_2_pw3 * d_12 * b - 2 * p_1 * p_2 * d_12_pw2 * b; factors(4) = -2 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * b + f_2_pw2 * p_2_pw2 * d_12_pw2 + 2 * p_1_pw3 * d_12 - p_1_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw2 * p_1_pw2 - p_1_pw4 - 2 * f_2_pw2 * p_2_pw2 * p_1 * d_12 + p_2_pw2 * f_1_pw2 * p_1_pw2 + f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2; // Computation of roots Eigen::Matrix<double, 4, 1> realRoots; P3P::solveQuartic(factors, realRoots); // Backsubstitution of each solution for (int i = 0; i < 4; ++i) { double cot_alpha = (-f_1 * p_1 / f_2 - realRoots(i) * p_2 + d_12 * b) / (-f_1 * realRoots(i) * p_2 / f_2 + p_1 - d_12); double cos_theta = realRoots(i); double sin_theta = sqrt(1 - pow((double)realRoots(i), 2)); double sin_alpha = sqrt(1 / (pow(cot_alpha, 2) + 1)); double cos_alpha = sqrt(1 - pow(sin_alpha, 2)); if (cot_alpha < 0) { cos_alpha = -cos_alpha; } Eigen::Vector3d C; C(0) = d_12 * cos_alpha * (sin_alpha * b + cos_alpha); C(1) = cos_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha); C(2) = sin_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha); C = P1 + N.transpose() * C; Eigen::Matrix3d R; R(0, 0) = -cos_alpha; R(0, 1) = -sin_alpha * cos_theta; R(0, 2) = -sin_alpha * sin_theta; R(1, 0) = sin_alpha; R(1, 1) = -cos_alpha * cos_theta; R(1, 2) = -cos_alpha * sin_theta; R(2, 0) = 0; R(2, 1) = -sin_theta; R(2, 2) = cos_theta; R = N.transpose() * R.transpose() * T; Eigen::Matrix<double, 3, 4> solution; solution.block<3, 3>(0, 0) = R; solution.col(3) = C; solutions(i) = solution; } return 0; }
int main(int /*argc*/, char** /*argv*/) { try { imp::ImageCv32fC1::Ptr cv_im0; imp::cvBridgeLoad(cv_im0, "/home/mwerlberger/data/epipolar_stereo_test/00000.png", imp::PixelOrder::gray); imp::ImageCv32fC1::Ptr cv_im1; imp::cvBridgeLoad(cv_im1, "/home/mwerlberger/data/epipolar_stereo_test/00001.png", imp::PixelOrder::gray); // rectify images for testing imp::cu::ImageGpu32fC1::Ptr im0 = std::make_shared<imp::cu::ImageGpu32fC1>(*cv_im0); imp::cu::ImageGpu32fC1::Ptr im1 = std::make_shared<imp::cu::ImageGpu32fC1>(*cv_im1); Eigen::Quaterniond q_world_im0(0.14062777, 0.98558398, 0.02351040, -0.09107859); Eigen::Quaterniond q_world_im1(0.14118687, 0.98569744, 0.01930722, -0.08996696); Eigen::Vector3d t_world_im0(-0.12617580, 0.50447008, 0.15342121); Eigen::Vector3d t_world_im1(-0.11031053, 0.50314023, 0.15158643); imp::cu::PinholeCamera cu_cam(414.09, 413.799, 355.567, 246.337); // im0: fixed image; im1: moving image imp::cu::Matrix3f F_fix_mov; imp::cu::Matrix3f F_mov_fix; Eigen::Matrix3d F_fm, F_mf; { // compute fundamental matrix Eigen::Matrix3d R_world_mov = q_world_im1.matrix(); Eigen::Matrix3d R_world_fix = q_world_im0.matrix(); Eigen::Matrix3d R_fix_mov = R_world_fix.inverse()*R_world_mov; // in ref coordinates Eigen::Vector3d t_fix_mov = R_world_fix.inverse()*(-t_world_im0 + t_world_im1); Eigen::Matrix3d tx_fix_mov; tx_fix_mov << 0, -t_fix_mov[2], t_fix_mov[1], t_fix_mov[2], 0, -t_fix_mov[0], -t_fix_mov[1], t_fix_mov[0], 0; Eigen::Matrix3d E_fix_mov = tx_fix_mov * R_fix_mov; Eigen::Matrix3d K; K << cu_cam.fx(), 0, cu_cam.cx(), 0, cu_cam.fy(), cu_cam.cy(), 0, 0, 1; Eigen::Matrix3d Kinv = K.inverse(); F_fm = Kinv.transpose() * E_fix_mov * Kinv; F_mf = F_fm.transpose(); } // end .. compute fundamental matrix // convert the Eigen-thingy to something that we can use in CUDA for(size_t row=0; row<F_fix_mov.rows(); ++row) { for(size_t col=0; col<F_fix_mov.cols(); ++col) { F_fix_mov(row,col) = (float)F_fm(row,col); F_mov_fix(row,col) = (float)F_mf(row,col); } } // compute SE3 transformation imp::cu::SE3<float> T_world_fix( static_cast<float>(q_world_im0.w()), static_cast<float>(q_world_im0.x()), static_cast<float>(q_world_im0.y()), static_cast<float>(q_world_im0.z()), static_cast<float>(t_world_im0.x()), static_cast<float>(t_world_im0.y()), static_cast<float>(t_world_im0.z())); imp::cu::SE3<float> T_world_mov( static_cast<float>(q_world_im1.w()), static_cast<float>(q_world_im1.x()), static_cast<float>(q_world_im1.y()), static_cast<float>(q_world_im1.z()), static_cast<float>(t_world_im1.x()), static_cast<float>(t_world_im1.y()), static_cast<float>(t_world_im1.z())); imp::cu::SE3<float> T_mov_fix = T_world_mov.inv() * T_world_fix; // end .. compute SE3 transformation std::cout << "T_mov_fix:\n" << T_mov_fix << std::endl; std::unique_ptr<imp::cu::VariationalEpipolarStereo> stereo( new imp::cu::VariationalEpipolarStereo()); stereo->parameters()->verbose = 1; stereo->parameters()->solver = imp::cu::StereoPDSolver::EpipolarPrecondHuberL1; stereo->parameters()->ctf.scale_factor = 0.8f; stereo->parameters()->ctf.iters = 30; stereo->parameters()->ctf.warps = 5; stereo->parameters()->ctf.apply_median_filter = true; stereo->parameters()->lambda = 20; stereo->addImage(im0); stereo->addImage(im1); imp::cu::ImageGpu32fC1::Ptr cu_mu = std::make_shared<imp::cu::ImageGpu32fC1>(im0->size()); imp::cu::ImageGpu32fC1::Ptr cu_sigma2 = std::make_shared<imp::cu::ImageGpu32fC1>(im0->size()); cu_mu->setValue(-5.f); cu_sigma2->setValue(0.0f); stereo->setFundamentalMatrix(F_mov_fix); stereo->setIntrinsics({cu_cam, cu_cam}); stereo->setExtrinsics(T_mov_fix); stereo->setDepthProposal(cu_mu, cu_sigma2); stereo->solve(); std::shared_ptr<imp::cu::ImageGpu32fC1> d_disp = stereo->getDisparities(); { imp::Pixel32fC1 min_val,max_val; imp::cu::minMax(*d_disp, min_val, max_val); std::cout << "disp: min: " << min_val.x << " max: " << max_val.x << std::endl; } imp::cu::cvBridgeShow("im0", *im0); imp::cu::cvBridgeShow("im1", *im1); // *d_disp *= -1; { imp::Pixel32fC1 min_val,max_val; imp::cu::minMax(*d_disp, min_val, max_val); std::cout << "disp: min: " << min_val.x << " max: " << max_val.x << std::endl; } imp::cu::cvBridgeShow("disparities", *d_disp, -18.0f, 11.0f); imp::cu::cvBridgeShow("disparities minmax", *d_disp, true); cv::waitKey(); } catch (std::exception& e) { std::cout << "[exception] " << e.what() << std::endl; assert(false); } return EXIT_SUCCESS; }
Eigen::Matrix3d get() { Eigen::Matrix3d interim = worker.get(); Eigen::Matrix3d retval = interim.transpose(); return retval; }