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;
}
