Skip to content

Commit

Permalink
Integrated color info to Frame
Browse files Browse the repository at this point in the history
  • Loading branch information
@ParikaGoel
ParikaGoel committed Jul 19, 2019
1 parent 15bb48b commit 431b526
Show file tree
Hide file tree
Showing 8 changed files with 65 additions and 55 deletions.
6 changes: 5 additions & 1 deletion FusionLib/include/Frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include <cmath>

#include <vector>
#include "data_types.h"
#include <Eigen.h>

#ifndef MINF
Expand All @@ -16,7 +17,7 @@ class Frame {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Frame(const double* depthMap, const Eigen::Matrix3d& depthIntrinsics,
Frame(const double* depthMap, const BYTE* colorMap, const Eigen::Matrix3d& depthIntrinsics,
const unsigned int width, const unsigned int height, double maxDistance = 2);

bool WriteMesh(const std::string& filename, std::string color);
Expand All @@ -37,6 +38,8 @@ class Frame {

const std::vector<double>& getDepthMap() const;

const std::vector<Vector4uc>& getColorMap() const;

const Eigen::Matrix3d& getIntrinsics() const;

const unsigned int getWidth() const;
Expand Down Expand Up @@ -71,4 +74,5 @@ class Frame {
Eigen::Matrix4d m_global_pose;
Eigen::Matrix3d m_intrinsic_matrix;
std::vector<double> m_depth_map;
std::vector<Vector4uc> m_color_map;
};
8 changes: 4 additions & 4 deletions FusionLib/include/Volume.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <Eigen/Dense>
#include <vector>
#include <utility>
#include "data_types.h"

class Ray
{
Expand All @@ -19,11 +20,10 @@ class Volume {

bool intersects(const Ray &r, double& entry_distance) const;

std::vector<std::pair<double,double>> &getTSDFData() ;
std::vector<Voxel>& getVoxelData();

const Eigen::Vector3d &getOrigin() const;


const Eigen::Vector3i &getVolumeSize() const;

float getVoxelScale() const;
Expand All @@ -34,8 +34,8 @@ class Volume {
Eigen::Vector3d getTSDFGrad(Eigen::Vector3d global);

private:
//_tsdfData contains tuples of tsdf & Weight
std::vector<std::pair<double,double>> _tsdfData;
//_voxelData contains tuples of tsdf & Weight
std::vector<Voxel> _voxelData;
const Eigen::Vector3i _volumeSize;
const double _voxelScale;
const Eigen::Vector3d _volumeRange;
Expand Down
12 changes: 12 additions & 0 deletions FusionLib/include/data_types.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
#pragma once

#include "Eigen.h"

typedef unsigned char BYTE;

struct Voxel{
double tsdf;
double weight;
Vector4uc color;
Voxel():tsdf(0.0f),weight(0.0f),color(0,0,0,0){}
};
13 changes: 11 additions & 2 deletions FusionLib/src/Frame.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,18 @@
#include <iostream>


Frame::Frame(const double * depthMap, const Eigen::Matrix3d &depthIntrinsics,
Frame::Frame(const double * depthMap, const BYTE* colorMap, const Eigen::Matrix3d &depthIntrinsics,
const unsigned int width, const unsigned int height, double maxDistance)
: m_width(width),m_height(height),m_intrinsic_matrix(depthIntrinsics){

m_depth_map = std::vector<double>(width*height);
for (size_t i = 0; i < width*height; ++i)
m_depth_map[i] = depthMap[i];

m_color_map = std::vector<Vector4uc>(width*height);
for (size_t i = 0; i < width*height; i++)
m_color_map[i] = Vector4uc(colorMap[i*4],colorMap[i*4+1],colorMap[i*4+2],colorMap[i*4+3]);

auto pointsTmp = computeCameraCoordinates(width, height);
auto normalsTmp = computeNormals(pointsTmp, width, height, maxDistance);

Expand Down Expand Up @@ -79,7 +83,8 @@ bool Frame::WriteMesh(const std::string& filename, std::string color) {
const auto& vertex = m_points_global[i];
if (vertex.allFinite())
outFile << vertex.x() << " " << vertex.y() << " " << vertex.z() << " "
<< color << std::endl;
<< m_color_map[i][0] << " " << m_color_map[i][1] << " "
<< m_color_map[i][2] << " " << m_color_map[i][3] << std::endl;
else
outFile << "0.0 0.0 0.0 0 0 0 0" << std::endl;
}
Expand Down Expand Up @@ -217,6 +222,10 @@ const std::vector<double>& Frame::getDepthMap() const{
return m_depth_map;
}

const std::vector<Vector4uc>& Frame::getColorMap() const{
return m_color_map;
}

const Eigen::Matrix3d& Frame::getIntrinsics() const{
return m_intrinsic_matrix;
}
Expand Down
10 changes: 5 additions & 5 deletions FusionLib/src/Fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ bool Fusion::reconstructSurface(const std::shared_ptr<Frame>& currentFrame,const
auto pose = currentFrame->getGlobalPose().inverse();
auto width = currentFrame->getWidth();
auto height = currentFrame->getHeight();
auto tsdfVolumeData = volume->getTSDFData();
auto tsdfVolumeData = volume->getVoxelData();

for (int z = 0;z<volumeSize.z();z++) {
for( int y =0;y<volumeSize.y();y++){
Expand Down Expand Up @@ -41,15 +41,15 @@ bool Fusion::reconstructSurface(const std::shared_ptr<Frame>& currentFrame,const
const double current_tsdf = std::min(1., sdf / truncationDistance); // *sgn(sdf)
const double current_weight = 1.0;
size_t tsdf_index = x+(y*volumeSize.x())+(z*volumeSize.x()*volumeSize.y());
const double old_tsdf=tsdfVolumeData[tsdf_index].first;
const double old_weight = tsdfVolumeData[tsdf_index].second;
const double old_tsdf=tsdfVolumeData[tsdf_index].tsdf;
const double old_weight = tsdfVolumeData[tsdf_index].weight;

const double updated_tsdf = (old_weight*old_tsdf + current_weight*current_tsdf)/
(old_weight+current_weight);
const double updated_weight = old_weight+current_weight;

tsdfVolumeData[tsdf_index].first = updated_tsdf;
tsdfVolumeData[tsdf_index].second = updated_weight;
tsdfVolumeData[tsdf_index].tsdf = updated_tsdf;
tsdfVolumeData[tsdf_index].weight = updated_weight;

}
}
Expand Down
15 changes: 0 additions & 15 deletions FusionLib/src/VirtualSensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,23 +61,8 @@ bool VirtualSensor::processNextFrame() {
m_depthFrame[i] = MINF;
else
m_depthFrame[i] = dImage.data[i] * 1.0f / 5000.0f;
//m_depthFrame[i] = dImage.data[i] * 0.000125f;
// m_depthFrame[i] = dImage.data[i];
}

// find transformation (simple nearest neighbor, linear search)
// double timestamp = m_depthImagesTimeStamps[m_currentIdx];
// double min = std::numeric_limits<double>::max();
// int idx = 0;
// for (unsigned int i = 0; i < m_trajectory.size(); ++i) {
// double d = abs(m_trajectoryTimeStamps[i] - timestamp);
// if (min > d) {
// min = d;
// idx = i;
// }
// }
// m_currentTrajectory = m_trajectory[idx];

return true;
}

Expand Down
38 changes: 19 additions & 19 deletions FusionLib/src/Volume.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,16 +11,16 @@ Ray::Ray(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir) : orig(origi


Volume::Volume(const Eigen::Vector3d origin, const Eigen::Vector3i volumeSize, const double voxelScale)
: _tsdfData(), _volumeSize(volumeSize),
: _voxelData(), _volumeSize(volumeSize),
_voxelScale(voxelScale), _origin(origin),
_maxPoint(voxelScale * volumeSize.cast<double>()),
_volumeRange(volumeSize.cast<double>()*voxelScale)
{
_tsdfData.reserve(volumeSize.x() * volumeSize.y() * volumeSize.z());
_voxelData.reserve(volumeSize.x() * volumeSize.y() * volumeSize.z());
for (int z = 0;z<volumeSize.z();z++)
for( int y =0;y<volumeSize.y();y++)
for(int x=0;x< volumeSize.x();x++)
_tsdfData.emplace_back(std::pair<double,double>(0,0));
_voxelData.emplace_back(Voxel());

Eigen::Vector3d half_voxelSize(voxelScale/2, voxelScale/2, voxelScale/2);
bounds[0] = _origin + half_voxelSize;
Expand Down Expand Up @@ -73,8 +73,8 @@ const Eigen::Vector3d& Volume::getOrigin() const{
}


std::vector<std::pair<double, double>> &Volume::getTSDFData() {
return _tsdfData;
std::vector<Voxel>& Volume::getVoxelData() {
return _voxelData;
}

const Eigen::Vector3i &Volume::getVolumeSize() const {
Expand All @@ -101,9 +101,9 @@ double Volume::getTSDF(Eigen::Vector3d global){
currentPosition.y() = int(shifted.y());
currentPosition.z() = int(shifted.z());

std::pair<double,double> fusionPoints= getTSDFData()[currentPosition.x() + currentPosition.y()*_volumeSize.x()
Voxel voxelData = getVoxelData()[currentPosition.x() + currentPosition.y()*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()];
return fusionPoints.first;
return voxelData.tsdf;
}

Eigen::Vector3d Volume::getTSDFGrad(Eigen::Vector3d global){
Expand All @@ -115,18 +115,18 @@ Eigen::Vector3d Volume::getTSDFGrad(Eigen::Vector3d global){

// TODO: double check

double tsdf_x0 = getTSDFData()[(currentPosition.x()-1) + currentPosition.y()*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()].first;
double tsdf_x1 = getTSDFData()[(currentPosition.x()+1) + currentPosition.y()*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()].first;
double tsdf_y0 = getTSDFData()[currentPosition.x() + (currentPosition.y()-1)*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()].first;
double tsdf_y1 = getTSDFData()[currentPosition.x() + (currentPosition.y()+1)*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()].first;
double tsdf_z0 = getTSDFData()[currentPosition.x() + currentPosition.y()*_volumeSize.x()
+ (currentPosition.z()-1)*_volumeSize.x()*_volumeSize.y()].first;
double tsdf_z1 = getTSDFData()[currentPosition.x() + currentPosition.y()*_volumeSize.x()
+ (currentPosition.z()+1)*_volumeSize.x()*_volumeSize.y()].first;
double tsdf_x0 = getVoxelData()[(currentPosition.x()-1) + currentPosition.y()*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()].tsdf;
double tsdf_x1 = getVoxelData()[(currentPosition.x()+1) + currentPosition.y()*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()].tsdf;
double tsdf_y0 = getVoxelData()[currentPosition.x() + (currentPosition.y()-1)*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()].tsdf;
double tsdf_y1 = getVoxelData()[currentPosition.x() + (currentPosition.y()+1)*_volumeSize.x()
+ currentPosition.z()*_volumeSize.x()*_volumeSize.y()].tsdf;
double tsdf_z0 = getVoxelData()[currentPosition.x() + currentPosition.y()*_volumeSize.x()
+ (currentPosition.z()-1)*_volumeSize.x()*_volumeSize.y()].tsdf;
double tsdf_z1 = getVoxelData()[currentPosition.x() + currentPosition.y()*_volumeSize.x()
+ (currentPosition.z()+1)*_volumeSize.x()*_volumeSize.y()].tsdf;

return Eigen::Vector3d(tsdf_x1 - tsdf_x0, tsdf_y1 - tsdf_y0, tsdf_z1 - tsdf_z0) / (_voxelScale*2);
}
18 changes: 9 additions & 9 deletions main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ bool process_frame( size_t frame_cnt, std::shared_ptr<Frame> prevFrame,std::shar
throw "ICP Pose Estimation failed";
};

// STEP 2: Surface reconstruction
/*// STEP 2: Surface reconstruction
//TODO does icp.estimate set the new Pose to currentFrame? Otherwise it needs to be added as function parameter
Fusion fusion;
if(!fusion.reconstructSurface(currentFrame,volume, config.m_truncationDistance)){
Expand All @@ -66,7 +66,7 @@ bool process_frame( size_t frame_cnt, std::shared_ptr<Frame> prevFrame,std::shar
Raycast raycast;
if(!raycast.surfacePrediction(currentFrame,volume, config.m_truncationDistance)){
throw "Raycasting failed";
};
};*/
return true;
}

Expand Down Expand Up @@ -112,24 +112,24 @@ int main(){
const double* depthMap = &sensor.getDepth()[0];
BYTE* colors = &sensor.getColorRGBX()[0];

std::shared_ptr<Frame> prevFrame = std::make_shared<Frame>(Frame(depthMap, depthIntrinsics, depthWidth, depthHeight));
std::shared_ptr<Frame> prevFrame = std::make_shared<Frame>(Frame(depthMap, colors, depthIntrinsics, depthWidth, depthHeight));
prevFrame->WriteMesh(filenameBaseOut+"0.off", "0 0 255 100");

std::cout << std::endl;
auto map = prevFrame->getDepthMap();
/*auto map = prevFrame->getDepthMap();
MeshWriter::toFile( "my_mesh", colors, prevFrame->getPoints());
MeshWriter::toFile( "my_mesh", colors, prevFrame->getPoints());*/

int i = 0;
int i = 1;
const int iMax = 45;

std::stringstream ss;
ss << filenameBaseOut << i << ".off";
prevFrame->WriteMesh(ss.str(), "0 0 255 100");

while(sensor.processNextFrame() && i <= iMax){

const double* depthMap = &sensor.getDepth()[0];
std::shared_ptr<Frame> currentFrame = std::make_shared<Frame>(Frame(depthMap,depthIntrinsics, depthWidth, depthHeight));
BYTE* colors = &sensor.getColorRGBX()[0];
std::shared_ptr<Frame> currentFrame = std::make_shared<Frame>(Frame(depthMap, colors, depthIntrinsics, depthWidth, depthHeight));
process_frame(i,prevFrame,currentFrame,volume,config);

ss << filenameBaseOut << i << ".off";
Expand Down

0 comments on commit 431b526

Please sign in to comment.