void SgMesh::transform(const Affine3f& T) { if(hasVertices()){ auto& v = *vertices(); for(size_t i=0; i < v.size(); ++i){ v[i] = T.linear() * v[i] + T.translation(); } if(hasNormals()){ auto& n = *normals(); for(size_t i=0; i < n.size(); ++i){ n[i] = T.linear() * n[i]; } } } setPrimitive(MESH); // clear the primitive information }
void pcl::gpu::RayCaster::run(const TsdfVolume& volume, const Affine3f& camera_pose) { camera_pose_.linear() = camera_pose.linear(); camera_pose_.translation() = camera_pose.translation(); volume_size_ = volume.getSize(); device::Intr intr (fx_, fy_, cx_, cy_); vertex_map_.create(rows * 3, cols); normal_map_.create(rows * 3, cols); typedef Matrix<float, 3, 3, RowMajor> Matrix3f; Matrix3f R = camera_pose_.linear(); Vector3f t = camera_pose_.translation(); const Mat33& device_R = device_cast<const Mat33>(R); const float3& device_t = device_cast<const float3>(t); float tranc_dist = volume.getTsdfTruncDist(); device::raycast (intr, device_R, device_t, tranc_dist, device_cast<const float3>(volume_size_), volume.data(), vertex_map_, normal_map_); }
bool kfusion::KinFu::operator()(const kfusion::cuda::Depth& depth, const kfusion::cuda::Image& /*image*/) { const KinFuParams& p = params_; const int LEVELS = icp_->getUsedLevelsNum(); cuda::computeDists(depth, dists_, p.intr); cuda::depthBilateralFilter(depth, curr_.depth_pyr[0], p.bilateral_kernel_size, p.bilateral_sigma_spatial, p.bilateral_sigma_depth); if (p.icp_truncate_depth_dist > 0) kfusion::cuda::depthTruncation(curr_.depth_pyr[0], p.icp_truncate_depth_dist); for (int i = 1; i < LEVELS; ++i) cuda::depthBuildPyramid(curr_.depth_pyr[i-1], curr_.depth_pyr[i], p.bilateral_sigma_depth); for (int i = 0; i < LEVELS; ++i) #if defined USE_DEPTH cuda::computeNormalsAndMaskDepth(p.intr, curr_.depth_pyr[i], curr_.normals_pyr[i]); #else cuda::computePointNormals(p.intr(i), curr_.depth_pyr[i], curr_.points_pyr[i], curr_.normals_pyr[i]); #endif cuda::waitAllDefaultStream(); //can't perform more on first frame if (frame_counter_ == 0) { volume_->integrate(dists_, cyclical_.getBuffer(), poses_.back(), p.intr); #if defined USE_DEPTH curr_.depth_pyr.swap(prev_.depth_pyr); #else curr_.points_pyr.swap(prev_.points_pyr); #endif curr_.normals_pyr.swap(prev_.normals_pyr); return ++frame_counter_, false; } /////////////////////////////////////////////////////////////////////////////////////////// // ICP Affine3f affine; // cuur -> prev { //ScopeTime time("icp"); #if defined USE_DEPTH bool ok = icp_->estimateTransform(affine, p.intr, curr_.depth_pyr, curr_.normals_pyr, prev_.depth_pyr, prev_.normals_pyr); #else bool ok = icp_->estimateTransform(affine, p.intr, curr_.points_pyr, curr_.normals_pyr, prev_.points_pyr, prev_.normals_pyr); #endif if (!ok) return reset(), false; } poses_.push_back(poses_.back() * affine); // curr -> global // check if we need to shift if(checkForShift_) has_shifted_ = cyclical_.checkForShift(volume_, getCameraPose(), params_.distance_camera_target , perform_shift_, perform_last_scan_, record_mode_); perform_shift_ = false; /////////////////////////////////////////////////////////////////////////////////////////// // Volume integration Affine3f local_pose = Affine3f().translate(poses_.back().translation() - volume_->getPose().translation()); local_pose.rotate(poses_.back().rotation()); // We do not integrate volume if camera does not move. float rnorm = (float)cv::norm(affine.rvec()); float tnorm = (float)cv::norm(affine.translation()); bool integrate = (rnorm + tnorm)/2 >= p.tsdf_min_camera_movement; if (integrate) { //ScopeTime time("tsdf"); volume_->integrate(dists_, cyclical_.getBuffer(), poses_.back(), p.intr); } /////////////////////////////////////////////////////////////////////////////////////////// // Ray casting { //ScopeTime time("ray-cast-all"); #if defined USE_DEPTH volume_->raycast(poses_.back(), cyclical_.getBuffer(), p.intr, prev_.depth_pyr[0], prev_.normals_pyr[0]); for (int i = 1; i < LEVELS; ++i) resizeDepthNormals(prev_.depth_pyr[i-1], prev_.normals_pyr[i-1], prev_.depth_pyr[i], prev_.normals_pyr[i]); #else //volume_->raycast(local_pose, p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]); volume_->raycast(poses_.back(), cyclical_.getBuffer(), p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]); for (int i = 1; i < LEVELS; ++i) resizePointsNormals(prev_.points_pyr[i-1], prev_.normals_pyr[i-1], prev_.points_pyr[i], prev_.normals_pyr[i]); #endif cuda::waitAllDefaultStream(); } return ++frame_counter_, true; }
int main(int argc, char** argv) { MatrixXf m(3,3); m << 1,2,3,4,5,6,7,8,9; Vector3f v = m.col(0); Vector3f v2 = m.col(1); Vector3f v3 = m.col(2); cout<<"Mat = "<<m<<endl; cout<<"vector = " << v<<endl; MatrixXf n = (m.rowwise() -= v); cout<<"Mat2 = "<<n<<endl; Matrix3f r; r.col(0) = v; r.col(1) = v2; r.col(2) = v3; cout<<"Mat3 = "<<r<<endl; r *= 2; cout<<"Mat3 = "<<r<<endl; cout<<"Mat3 = "<<m<<endl; float arr[] = {1,2,3}; vector<float> w(arr, arr + sizeof(arr) / sizeof(float)); for(int i = 0; i < w.size(); i+=1) cout<<w[i]<<"\t"; cout<<endl<<"---------------"<<endl; /** Inverting a singular matrix. **/ m << 1,0,0,1,0,0,1,0,0; cout<<" Should segfault/ get garbage: "<<m.determinant()<<endl; /** Affine matrix in Eigen. */ MatrixXf ml(4,4); ml << 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16; Affine3f t; Vector3f d(10,20,30); t.linear() = ml.transpose().block(0,0,3,3); t.translation() = ml.block(0,3,3,1); cout<<"Affine: "<<endl<<t.affine()<<endl; Vector3f tt(1,2,3); t.translation() += tt; cout<<"Affine after translation: "<<endl<<t.affine()<<endl; /** Blocks. */ MatrixXf matr(3,2); matr << 1,2,3,4,5,6; MatrixXf combo(4,2); combo <<matr, MatrixXf::Ones(1,matr.cols()); cout<<"Matr = "<<matr<<endl; cout<<"Comb = "<<combo<<endl; MatrixXf rrr = combo.block(0,0,3,2); cout<<"rrr = "<<rrr<<endl; /** Filling up a matrix*/ std::cout<<"---------------------------------------"<<std::endl; MatrixXf matF(5,3); Vector3f vF(1,.3,3); Vector3f vF1(.123,.2,.3); Vector3f vF2(33.4,23.3,12.07); Vector3f vF3(0.54,8.96,14.3); Vector3f vF4(8.9,0.34,32.2); matF.row(0) = vF; matF.row(1) = vF1; matF.row(2) = vF2; matF.row(3) = vF3; matF.row(4) = vF4; RowVector3f means = matF.colwise().sum()/5; MatrixXf centered = (matF.rowwise() - means); cout<<"Matrix Fill = \n"<<matF<<std::endl; cout<<"Means = \n"<<means<<std::endl; cout<<"Centered = \n"<<centered<<std::endl; Eigen::JacobiSVD<MatrixXf> svd(centered / sqrt(5), ComputeFullV); cout << "Its singular values are:" << endl << svd.singularValues() << endl; cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl; MatrixXf V = svd.matrixV(); Vector3f f1 = V.col(0); Vector3f f2 = V.col(1); Vector3f f3 = V.col(2); cout << "f1(0)" << f1(0) << endl; VectorXf faa(V.rows()); faa.setZero(); //for (int i= 0; i < V.rows(); i++) V.col(2) = faa; cout << "V " << V<< endl; cout << "<v1,v2>" << f1.dot(f2) << endl; cout << "<v1,v3>" << f1.dot(f3) << endl; cout << "<v3,v2>" << f3.dot(f2) << endl; std::cout<<"---------------------------------------"<<std::endl; Vector3f o3(1,2,3), o2(4,5,6); cout << "outer? : "<< o3.cwiseProduct(o2) <<endl; MatrixXd mxx(3,3); mxx << 1,0,0,0,2,0,0,0,3; cout << "mxx : "<< endl<<mxx <<endl; vector<double> v31(3); VectorXd::Map(&v31[0], 3) = mxx.row(0); cout << "v: "<<endl; for(int i=0; i < 3; i++) cout<< v31[i]<< " " <<endl; cout<<endl; v31[1] = 100; cout << "v: "<<endl; for(int i=0; i < 3; i++) cout << v31[i]<< " " <<endl; cout<<endl; cout << " mxx : "<<endl <<mxx<<endl; Matrix3d tmat = Matrix3d::Identity(); tmat(0,0) = 0; tmat(1,1) = 10; tmat(1,2) = 20; cout << "tmat: " << tmat<<endl; cout << "------------\n"; for (unsigned i=0; i < tmat.rows(); i++) { double sum = tmat.row(i).sum() + numeric_limits<double>::min(); tmat.row(i) /= sum; } cout << tmat << endl; MatrixXd src(4,3); src<< 0,0,0,1,1,1,2,2,2,3,3,3; MatrixXd target(1,3); target<< 1,1,1; cout <<"cloud err: "<< (src.rowwise() -target.row(0)).rowwise().squaredNorm().transpose()<<endl; return 0; }