void
GLWindow::init()
{
/* Use depth buffering for hidden surface elimination. */
glEnable(GL_DEPTH_TEST);
camera_.setFovY(3.14f / 4);
camera_.setPosition(Vector3f(0, 0, -5));
camera_.setTarget(Vector3f(0, 0, 0));
setView();
glewInit();
if (GLEW_ARB_vertex_shader && GLEW_ARB_fragment_shader)
std::cout << ("Ready for GLSL\n");
}
Camera::Camera() {
//mtx_ = new std::mutex();
fov_current_ = 60.0f;
fov_future_ = 60.0f;
aspect_ = 1.0f;
depth_near_ = 1.0f;
depth_far_ = 1000.f;
roll_correction_ = true;
always_update_ = false;
rotation_current_ = AngleAxis<float>(0, Vector3f(1.0f, 0.0f, 0.0f));
rotation_future_ = AngleAxis<float>(0, Vector3f(1.0f, 0.0f, 0.0f));
translation_current_ = Vector3f(0, 0, 0);
translation_future_ = Vector3f(0, 0, 0);
translation_speed_ = 1;
projection_dirty_ = true;
timer_ = new QTimer();
timer_->connect(timer_, &QTimer::timeout, [&] () {
Eigen::Vector3f trans_diff = translation_future_ - translation_current_;
bool good_enough_rotation = rotation_current_.isApprox(rotation_future_);
float fov_diff = fov_future_ - fov_current_;
if(trans_diff.norm() > 1e-4 || !good_enough_rotation || fabs(fov_diff) > 1e-10) {
translation_current_ = translation_current_ + trans_diff * 0.5;
rotation_current_ = rotation_current_.slerp(0.5, rotation_future_);
fov_current_ = fov_future_ * 0.5 + fov_current_ * 0.5;
if(fabs(fov_diff) > 1e-10)
projection_dirty_ = true;
emit updated();
}
else {
if(always_update_)
emit updated();
translation_speed_ = 1;
}
});
timer_->start(1000/60);
}
void MulticamFusion::integrateTSDF(const DepthMap& depth_raw, Matrix3frm* rotPtr, Vector3f* transPtr){
Matrix3frm init_Rcam;
Vector3f init_tcam;
if(rotPtr == 0 && transPtr == 0){
//Default Pose
init_Rcam = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
//init_tcam = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
//init_tcam = Vector3f(1.5f, 1.5f, -0.3f);
init_tcam = Vector3f(0, 0, -0.0f);
}
else{
init_Rcam = *rotPtr; // [Ri|ti] - pos of camera, i.e.
init_tcam = *transPtr; // transform from camera to global coo space for (i-1)th camera pose
}
Mat33& device_Rcam = device_cast<Mat33> (init_Rcam);
float3& device_tcam = device_cast<float3>(init_tcam);
Matrix3frm init_Rcam_inv = init_Rcam.inverse ();
Mat33& device_Rcam_inv = device_cast<Mat33> (init_Rcam_inv);
float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());
device::Intr intr (525.f, 525.f, 0, 0);
device::integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam,
tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), depthRawScaled_);
}
pcl::gpu::KinfuTracker::KinfuTracker (int rows, int cols) : rows_(rows), cols_(cols), global_time_(0), max_icp_distance_(0), integration_metric_threshold_(0.f), disable_icp_(false)
{
const Vector3f volume_size = Vector3f::Constant (VOLUME_SIZE);
const Vector3i volume_resolution(VOLUME_X, VOLUME_Y, VOLUME_Z);
tsdf_volume_ = TsdfVolume::Ptr( new TsdfVolume(volume_resolution) );
tsdf_volume_->setSize(volume_size);
setDepthIntrinsics (KINFU_DEFAULT_DEPTH_FOCAL_X, KINFU_DEFAULT_DEPTH_FOCAL_Y); // default values, can be overwritten
init_Rcam_ = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
init_tcam_ = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
const int iters[] = {10, 5, 4};
std::copy (iters, iters + LEVELS, icp_iterations_);
const float default_distThres = 0.10f; //meters
const float default_angleThres = sin (20.f * 3.14159254f / 180.f);
const float default_tranc_dist = 0.03f; //meters
setIcpCorespFilteringParams (default_distThres, default_angleThres);
tsdf_volume_->setTsdfTruncDist (default_tranc_dist);
allocateBufffers (rows, cols);
rmats_.reserve (30000);
tvecs_.reserve (30000);
reset ();
}
pcl::gpu::kinfuLS::KinfuTracker::KinfuTracker (const Eigen::Vector3f &volume_size, const float shiftingDistance, int rows, int cols) :
cyclical_( DISTANCE_THRESHOLD, VOLUME_SIZE, VOLUME_X), rows_(rows), cols_(cols), global_time_(0), max_icp_distance_(0), integration_metric_threshold_(0.f), perform_last_scan_ (false), finished_(false), lost_ (false), disable_icp_ (false)
{
//const Vector3f volume_size = Vector3f::Constant (VOLUME_SIZE);
const Vector3i volume_resolution (VOLUME_X, VOLUME_Y, VOLUME_Z);
volume_size_ = volume_size(0);
tsdf_volume_ = TsdfVolume::Ptr ( new TsdfVolume(volume_resolution) );
tsdf_volume_->setSize (volume_size);
shifting_distance_ = shiftingDistance;
// set cyclical buffer values
cyclical_.setDistanceThreshold (shifting_distance_);
cyclical_.setVolumeSize (volume_size_, volume_size_, volume_size_);
setDepthIntrinsics (FOCAL_LENGTH, FOCAL_LENGTH); // default values, can be overwritten
init_Rcam_ = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
init_tcam_ = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
const int iters[] = {10, 5, 4};
std::copy (iters, iters + LEVELS, icp_iterations_);
const float default_distThres = 0.10f; //meters
const float default_angleThres = sin (20.f * 3.14159254f / 180.f);
const float default_tranc_dist = 0.03f; //meters
setIcpCorespFilteringParams (default_distThres, default_angleThres);
tsdf_volume_->setTsdfTruncDist (default_tranc_dist);
allocateBufffers (rows, cols);
rmats_.reserve (30000);
tvecs_.reserve (30000);
reset ();
// initialize cyclical buffer
cyclical_.initBuffer(tsdf_volume_);
last_estimated_rotation_= Eigen::Matrix3f::Identity ();
last_estimated_translation_= volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
}
void
GLWindow::motion(int x, int y)
{
Mouse m = mouse_;
float delta_x = m.beginx - x, delta_y = m.beginy - y;
if (m.button == GLUT_LEFT_BUTTON && m.state == GLUT_DOWN)
{
if (m.modifiers & GLUT_ACTIVE_SHIFT)
{
//zoom in and out
camera_.setFovY(camera_.fovY() * (1.0f + (delta_y / camera_.vpWidth())));
}
else
{
Eigen::AngleAxisf ry(-delta_x / camera_.vpWidth(), Eigen::Vector3f(0, 1, 0));
Eigen::AngleAxisf rx(-delta_y / camera_.vpHeight(), Eigen::Vector3f(1, 0, 0));
Eigen::Quaternionf qx(rx), qy(ry);
camera_.rotateAroundTarget(qy);
camera_.rotateAroundTarget(qx);
}
}
else if (m.button == GLUT_MIDDLE_BUTTON && m.state == GLUT_DOWN)
{
Vector3f X = camera_.position();
Eigen::Quaternionf q = camera_.orientation();
float factor = 10;
Vector3f dX;
if (m.modifiers & GLUT_ACTIVE_SHIFT)
{
//move along the camera plane normal
dX = q * Vector3f(factor * delta_x / camera_.vpWidth(), 0, factor * (-delta_y) / camera_.vpHeight());
}
else
{
//compute the delta x using the the camera orientation,
//so that the motion is in the plane of the camera.
dX = q * Vector3f(factor * delta_x / camera_.vpWidth(), factor * (-delta_y) / camera_.vpHeight(), 0);
}
camera_.setPosition(X + dX);
}
mouse_.beginx = x;
mouse_.beginy = y;
}
void MulticamFusion::show(Eigen::Affine3f* pose_ptr){
device::Intr intr (525.f, 525.f, 0, 0);
//Ray-Casting
int pyr_rows = 480; int pyr_cols = 640;
depthRawScaled_.create (480, 640);
vmaps_g_prev_.create (pyr_rows*3, pyr_cols);
nmaps_g_prev_.create (pyr_rows*3, pyr_cols);
view_device_.create (480, 640);
if(pose_ptr != 0){
raycaster_ptr_->run(*tsdf_volume_, *pose_ptr);
raycaster_ptr_->generateSceneView(view_device_);
}
else{
//Generate Images
Eigen::Vector3f light_source_pose = tsdf_volume_->getSize() * (-3.f);
device::LightSource light;
light.number = 1;
light.pos[0] = device_cast<const float3>(light_source_pose);
float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());
//Viewer's Pose
Matrix3frm tempR = Eigen::Matrix3f::Identity ();
//Vector3f tempT = Vector3f(1.5f, 1.5f, -0.3f);
Vector3f tempT = Vector3f(0.09f, 0.29f, 0.14f);
Mat33& device_Rcam = device_cast<Mat33> (tempR);
float3& device_tcam = device_cast<float3>(tempT);
raycast (intr, device_Rcam, device_tcam, tsdf_volume_->getTsdfTruncDist(), device_volume_size,
tsdf_volume_->data(), vmaps_g_prev_, nmaps_g_prev_);
generateImage (vmaps_g_prev_, nmaps_g_prev_, light, view_device_);
}
getLastFrameCloud(*cloud_device_);
int c;
cloud_device_->download (cloud_ptr_->points, c);
cloud_ptr_->width = cloud_device_->cols ();
cloud_ptr_->height = cloud_device_->rows ();
cloud_ptr_->is_dense = false;
cloud_viewer_.removeAllPointClouds ();
cloud_viewer_.addPointCloud<PointXYZ>(cloud_ptr_);
//For Getting Pose
cloud_viewer_.spinOnce();
//Display
int cols;
view_device_.download(viewer_host_, cols);
image_viewer_.showRGBImage ((unsigned char*)&viewer_host_[0], view_device_.cols (), view_device_.rows ());
image_viewer_.spinOnce();
}
void
pcl::gpu::kinfuLS::KinfuTracker::reset ()
{
cout << "in reset function!" << std::endl;
if (global_time_)
PCL_WARN ("Reset\n");
// dump current world to a pcd file
/*
if (global_time_)
{
PCL_INFO ("Saving current world to current_world.pcd\n");
pcl::io::savePCDFile<pcl::PointXYZI> ("current_world.pcd", *(cyclical_.getWorldModel ()->getWorld ()), true);
// clear world model
cyclical_.getWorldModel ()->reset ();
}
*/
// clear world model
cyclical_.getWorldModel ()->reset ();
global_time_ = 0;
rmats_.clear ();
tvecs_.clear ();
rmats_.push_back (init_Rcam_);
tvecs_.push_back (init_tcam_);
tsdf_volume_->reset ();
// reset cyclical buffer as well
cyclical_.resetBuffer (tsdf_volume_);
if (color_volume_) // color integration mode is enabled
color_volume_->reset ();
// reset estimated pose
last_estimated_rotation_= Eigen::Matrix3f::Identity ();
last_estimated_translation_= Vector3f (volume_size_, volume_size_, volume_size_) * 0.5f - Vector3f (0, 0, volume_size_ / 2 * 1.2f);
lost_=false;
has_shifted_=false;
}
int main() {
MatrixXf m(3,2);
m <<
1.0, 2.0,
3.0, 4.0,
5.0, 6.0
;
JacobiSVD<MatrixXf> svd(m, ComputeFullU | ComputeFullV);
auto e = svd.singularValues();
// TODO. e needs to be 3x2, what is the nicest way? Almost there, but this changes value positions.
//MatrixXf e(svd.singularValues().asDiagonal());
//e.resize(m.rows(), m.cols());
cout << "E" << endl << e << endl << endl;
auto u = svd.matrixU();
cout << "U" << endl << u << endl << endl;
cout << "UU'" << endl << u.adjoint() * u << endl << endl;
auto v = svd.matrixV();
cout << "V" << endl << v << endl << endl;
cout << "VV'" << endl << v * v.adjoint() << endl << endl;
//cout << "UEV" << endl << u * e * v << endl << endl;
cout << "least squares" << endl << svd.solve(Vector3f(1, 0, 0)) << endl << endl;
}
MulticamFusion::MulticamFusion(){
cloud_viewer_.setBackgroundColor (0, 0, 0);
cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 1);
cloud_viewer_.addCoordinateSystem (1.0);
cloud_viewer_.initCameraParameters ();
cloud_viewer_.camera_.clip[0] = 0.01;
cloud_viewer_.camera_.clip[1] = 10.01;
cloud_viewer_.addText ("H: print help", 2, 15, 20, 34, 135, 246);
//Initialize TSDF Volume
volume_size = Vector3f::Constant (4.4f);
volume_resolution = Vector3i(512, 512, 512);
tsdf_volume_ = TsdfVolume::Ptr( new TsdfVolume(volume_resolution) );
tsdf_volume_->setSize(volume_size);
image_viewer_.registerKeyboardCallback(keyboard_callback, (void*)this);
raycaster_ptr_ = RayCaster::Ptr( new RayCaster(480, 640) );
cloud_viewer_.addCube(volume_size*0.5, Eigen::Quaternionf::Identity(), volume_size(0), volume_size(1), volume_size(2));
cloud_viewer_.registerKeyboardCallback(keyboard_callback, (void*)this);
//init_tcam = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
Eigen::Affine3f default_pose;
default_pose.linear() = Eigen::Matrix3f::Identity ();
default_pose.translation() = Vector3f(1.5f, 1.5f, -0.3f);
cout << default_pose.translation() << endl;
setViewerPose(cloud_viewer_, default_pose);
rows = 480; cols = 640;
cloud_device_ = new DeviceArray2D<PointXYZ>(rows, cols);
cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
}
void Camera::birds_eye() {
rotation_future_ = AngleAxis<float>(0, Vector3f(1, 0, 0));
translation_future_ = 20 * -Vector3f::UnitZ();
}
inline bool
pcl::gpu::kinfuLS::KinfuTracker::performPairWiseICP(const Intr cam_intrinsics, Matrix3frm& resulting_rotation , Vector3f& resulting_translation)
{
// we assume that both v and n maps are in the same coordinate space
// initialize rotation and translation to respectively identity and zero
Matrix3frm previous_rotation = Eigen::Matrix3f::Identity ();
Matrix3frm previous_rotation_inv = previous_rotation.inverse ();
Vector3f previous_translation = Vector3f(0,0,0);
///////////////////////////////////////////////
// Convert pose to device type
Mat33 device_cam_rot_prev_inv;
float3 device_cam_trans_prev;
convertTransforms(previous_rotation_inv, previous_translation, device_cam_rot_prev_inv, device_cam_trans_prev);
// Initialize output pose to current pose (i.e. identity and zero translation)
Matrix3frm current_rotation = previous_rotation;
Vector3f current_translation = previous_translation;
///////////////////////////////////////////////
// Run ICP
{
//ScopeTime time("icp-all");
for (int level_index = LEVELS-1; level_index>=0; --level_index)
{
int iter_num = icp_iterations_[level_index];
// current vertex and normal maps
MapArr& vmap_curr = vmaps_curr_[level_index];
MapArr& nmap_curr = nmaps_curr_[level_index];
// previous vertex and normal maps
MapArr& vmap_prev = vmaps_prev_[level_index];
MapArr& nmap_prev = nmaps_prev_[level_index];
// no need to transform maps from global to local since they are both in camera coordinates
// run ICP for iter_num iterations (return false when lost)
for (int iter = 0; iter < iter_num; ++iter)
{
//CONVERT POSES TO DEVICE TYPES
// CURRENT LOCAL POSE
Mat33 device_current_rotation = device_cast<Mat33> (current_rotation);
float3 device_current_translation_local = device_cast<float3> (current_translation);
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
// call the ICP function (see paper by Kok-Lim Low "Linear Least-squares Optimization for Point-to-Plane ICP Surface Registration")
estimateCombined (device_current_rotation, device_current_translation_local, vmap_curr, nmap_curr, device_cam_rot_prev_inv, device_cam_trans_prev, cam_intrinsics (level_index),
vmap_prev, nmap_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
// checking nullspace
double det = A.determinant ();
if ( fabs (det) < 1e-15 || pcl_isnan (det) )
{
if (pcl_isnan (det)) cout << "qnan" << endl;
PCL_WARN ("ICP PairWise LOST...\n");
//reset ();
return (false);
}
Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
float alpha = result (0);
float beta = result (1);
float gamma = result (2);
// deduce incremental rotation and translation from ICP's results
Eigen::Matrix3f cam_rot_incremental = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
Vector3f cam_trans_incremental = result.tail<3> ();
//compose global pose
current_translation = cam_rot_incremental * current_translation + cam_trans_incremental;
current_rotation = cam_rot_incremental * current_rotation;
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// since raw depthmaps are quite noisy, we make sure the estimated transform is big enought to be taken into account
float rnorm = rodrigues2(current_rotation).norm();
float tnorm = (current_translation).norm();
const float alpha = 1.f;
bool integrate = (rnorm + alpha * tnorm)/2 >= integration_metric_threshold_ * 2.0f;
if(integrate)
{
resulting_rotation = current_rotation;
resulting_translation = current_translation;
}
else
{
resulting_rotation = Eigen::Matrix3f::Identity ();
resulting_translation = Vector3f(0,0,0);
}
// ICP has converged
return (true);
}
