diff --git a/applications/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp
index 62fbb7265f84bf03ffbe536aa30a2d9d8131828a..997aaaa645852a7a9c893a9c555074fff6cc22a4 100644
--- a/applications/gui/src/src_window.cpp
+++ b/applications/gui/src/src_window.cpp
@@ -72,87 +72,6 @@ Eigen::Matrix<T,4,4> lookAt
return res;
}
-class VirtualCameraView : public nanogui::ImageView {
- public:
- VirtualCameraView(nanogui::Widget *parent) : nanogui::ImageView(parent, 0) {
- src_ = nullptr;
- eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
- centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
- up_ = Eigen::Vector3f(0,1.0f,0);
- lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
- lerpSpeed_ = 0.4f;
- depth_ = false;
- }
-
- void setSource(Source *src) { src_ = src; }
-
- bool mouseButtonEvent(const nanogui::Vector2i &p, int button, bool down, int modifiers) {
- //LOG(INFO) << "Mouse move: " << p[0];
- if (src_ && down) {
- Eigen::Vector4f camPos = src_->point(p[0],p[1]);
- camPos *= -1.0f;
- Eigen::Vector4f worldPos = src_->getPose() * camPos;
- lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
- LOG(INFO) << "Depth at click = " << -camPos[2];
- }
- }
-
- bool keyboardEvent(int key, int scancode, int action, int modifiers) {
- LOG(INFO) << "Key press" << key << " - " << action;
- if (key == 81 || key == 83) {
- // TODO Should rotate around lookAt object, but requires correct depth
- Eigen::Quaternion<float> q; q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, up_);
- eye_ = (q * (eye_ - centre_)) + centre_;
- } else if (key == 84 || key == 82) {
- float scalar = (key == 84) ? 0.99f : 1.01f;
- eye_ = ((eye_ - centre_) * scalar) + centre_;
- }
- }
-
- void draw(NVGcontext *ctx) {
- //net_->broadcast("grab");
- if (src_) {
- cv::Mat rgb, depth;
- centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
- Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
-
- src_->setPose(viewPose);
- src_->grab();
- src_->getFrames(rgb, depth);
-
- if (depth_) {
- if (depth.rows > 0) {
- cv::Mat idepth;
- depth.convertTo(idepth, CV_8U, 255.0f / 10.0f); // TODO(nick)
- applyColorMap(idepth, idepth, cv::COLORMAP_JET);
- texture_.update(idepth);
- bindImage(texture_.texture());
- }
- } else {
- if (rgb.rows > 0) {
- texture_.update(rgb);
- bindImage(texture_.texture());
- }
- }
-
- screen()->performLayout(ctx);
- }
- ImageView::draw(ctx);
- }
-
- void setDepth(bool d) { depth_ = d; }
-
- private:
- Source *src_;
- GLTexture texture_;
- Eigen::Vector3f eye_;
- Eigen::Vector3f centre_;
- Eigen::Vector3f up_;
- Eigen::Vector3f lookPoint_;
- float lerpSpeed_;
- bool depth_;
-};
-
SourceWindow::SourceWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
: nanogui::Window(parent, "Source View"), ctrl_(ctrl) {
setLayout(new nanogui::GroupLayout());
diff --git a/applications/reconstruct/src/virtual_source.cpp b/applications/reconstruct/src/virtual_source.cpp
index 578b360907a04a8198395a778ca1073cd7ae94be..0bc4abf4a72b089b50b786bd2d64daa4d1e94c02 100644
--- a/applications/reconstruct/src/virtual_source.cpp
+++ b/applications/reconstruct/src/virtual_source.cpp
@@ -50,7 +50,8 @@ bool VirtualSource::grab() {
params.m_sensorDepthWorldMin = params_.minDepth;
params.m_sensorDepthWorldMax = params_.maxDepth;
- rays_->render(scene_->getHashData(), scene_->getHashParams(), params, host_->getPose());
+ // TODO(Nick) Use double precision pose here
+ rays_->render(scene_->getHashData(), scene_->getHashParams(), params, host_->getPose().cast<float>());
//unique_lock<mutex> lk(mutex_);
if (rays_->isIntegerDepth()) {
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 07f0c300f3672134f2a3de0cc3cf8549f359c322..a5a37b54f9297dd02e37b684d9f72027b4d203a0 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -42,40 +42,40 @@ static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
out = Mat(rows, cols, CV_32FC1);
for (int i = 0; i < rows * cols; ++i) {
- float sum = 0;
+ double sum = 0;
int good_values = 0;
// Calculate mean
for (int i_in = 0; i_in < in.size(); ++i_in) {
- float d = in[i_in].at<float>(i);
- if (d < 40.0f) {
+ double d = in[i_in].at<float>(i);
+ if (d < 40.0) {
good_values++;
sum += d;
}
}
if (good_values > 0) {
- sum /= (float)good_values;
+ sum /= (double)good_values;
// Calculate variance
- float var = 0.0f;
+ double var = 0.0;
for (int i_in = 0; i_in < in.size(); ++i_in) {
- float d = in[i_in].at<float>(i);
- if (d < 40.0f) {
- float delta = (d*1000.0f - sum*1000.0f);
+ double d = in[i_in].at<float>(i);
+ if (d < 40.0) {
+ double delta = (d*1000.0 - sum*1000.0);
var += delta*delta;
}
}
- if (good_values > 1) var /= (good_values-1);
- else var = 0.0f;
+ if (good_values > 1) var /= (double)(good_values-1);
+ else var = 0.0;
//LOG(INFO) << "VAR " << var;
- //if (var < varThresh) {
- out.at<float>(i) = sum;
- //} else {
- // out.at<float>(i) = 41.0f;
- //}
+ if (var < varThresh) {
+ out.at<float>(i) = (float)sum;
+ } else {
+ out.at<float>(i) = 41.0f;
+ }
} else {
out.at<float>(i) = 41.0f;
}
@@ -83,7 +83,7 @@ static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
}
static PointXYZ makePCL(Source *s, int x, int y) {
- Eigen::Vector4f p1 = s->point(x,y);
+ Eigen::Vector4d p1 = s->point(x,y);
PointXYZ pcl_p1;
pcl_p1.x = p1[0];
pcl_p1.y = p1[1];
@@ -159,7 +159,7 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
if (d1_value < 39.0f) {
// Make PCL points with specific depth value
pcl::PointXYZ p1;
- Eigen::Vector4f p1e = src_->point(x,y,d1_value);
+ Eigen::Vector4d p1e = src_->point(x,y,d1_value);
p1.x = p1e[0];
p1.y = p1e[1];
p1.z = p1e[2];
@@ -171,7 +171,7 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
if (d2_value < 39.0f) {
// Make PCL points with specific depth value
pcl::PointXYZ p2;
- Eigen::Vector4f p2e = targ_->point(x,y,d2_value);
+ Eigen::Vector4d p2e = targ_->point(x,y,d2_value);
p2.x = p2e[0];
p2.y = p2e[1];
p2.z = p2e[2];
@@ -212,7 +212,7 @@ void Correspondances::removeLast() {
log_.pop_back();
}
-double Correspondances::estimateTransform(Eigen::Matrix4f &T) {
+double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
@@ -222,7 +222,7 @@ double Correspondances::estimateTransform(Eigen::Matrix4f &T) {
return validate.validateTransformation(src_cloud_, targ_cloud_, T);
}
-double Correspondances::estimateTransform(Eigen::Matrix4f &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
+double Correspondances::estimateTransform(Eigen::Matrix4d &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
@@ -312,46 +312,46 @@ void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>
}
}
-void Correspondances::getFeatures3D(std::vector<Eigen::Vector4f> &f) {
+void Correspondances::getFeatures3D(std::vector<Eigen::Vector4d> &f) {
for (int i=0; i<src_feat_.size(); i++) {
- Eigen::Vector4f p;
+ Eigen::Vector4d p;
const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
p[0] = pp.x;
p[1] = pp.y;
p[2] = pp.z;
- p[3] = 1.0f;
+ p[3] = 1.0;
f.push_back(p);
}
}
-void Correspondances::getTransformedFeatures(std::vector<Eigen::Vector4f> &f) {
+void Correspondances::getTransformedFeatures(std::vector<Eigen::Vector4d> &f) {
for (int i=0; i<src_feat_.size(); i++) {
- Eigen::Vector4f p;
+ Eigen::Vector4d p;
const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
p[0] = pp.x;
p[1] = pp.y;
p[2] = pp.z;
- p[3] = 1.0f;
+ p[3] = 1.0;
f.push_back(transform_ * p);
}
}
void Correspondances::getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f) {
for (int i=0; i<src_feat_.size(); i++) {
- Eigen::Vector4f p;
+ Eigen::Vector4d p;
const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
p[0] = pp.x;
p[1] = pp.y;
p[2] = pp.z;
- p[3] = 1.0f;
+ p[3] = 1.0;
f.push_back(src_->point(transform_ * p));
}
}
-double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
+double Correspondances::findBestSubset(Eigen::Matrix4d &tr, int K, int N) {
double score = 10000.0f;
vector<int> best;
- Eigen::Matrix4f bestT;
+ Eigen::Matrix4d bestT;
std::mt19937 rng(std::chrono::steady_clock::now().time_since_epoch().count());
std::uniform_int_distribution<int> distribution(0,src_feat_.size()-1);
@@ -380,7 +380,7 @@ double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
tidx.push_back(targ_feat_[r]);
}
- Eigen::Matrix4f T;
+ Eigen::Matrix4d T;
float scoreT = estimateTransform(T, sidx, tidx);
if (scoreT < score) {
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index b708815c635fd23cbec5c32a052c1f501513184e..57f4442f6c893863fb2df146ec2ae13894691f9f 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -52,8 +52,8 @@ class Correspondances {
void removeLast();
const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
- void getFeatures3D(std::vector<Eigen::Vector4f> &f);
- void getTransformedFeatures(std::vector<Eigen::Vector4f> &f);
+ void getFeatures3D(std::vector<Eigen::Vector4d> &f);
+ void getTransformedFeatures(std::vector<Eigen::Vector4d> &f);
void getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f);
void setPoints(const std::vector<std::tuple<int,int,int,int>> &p) { log_ = p; }
@@ -63,13 +63,13 @@ class Correspondances {
*
* @return Validation score of the transform.
*/
- double estimateTransform(Eigen::Matrix4f &);
- double estimateTransform(Eigen::Matrix4f &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
+ double estimateTransform(Eigen::Matrix4d &);
+ double estimateTransform(Eigen::Matrix4d &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
- double findBestSubset(Eigen::Matrix4f &tr, int K, int N);
+ double findBestSubset(Eigen::Matrix4d &tr, int K, int N);
double icp();
- double icp(const Eigen::Matrix4f &T_in, Eigen::Matrix4f &T_out, const std::vector<int> &idx);
+ double icp(const Eigen::Matrix4d &T_in, Eigen::Matrix4d &T_out, const std::vector<int> &idx);
void convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2);
@@ -77,9 +77,9 @@ class Correspondances {
* Get the estimated transform. This includes any parent transforms to make
* it relative to root camera.
*/
- Eigen::Matrix4f transform();
+ Eigen::Matrix4d transform();
- void setTransform(Eigen::Matrix4f &t) { uptodate_ = true; transform_ = t; }
+ void setTransform(Eigen::Matrix4d &t) { uptodate_ = true; transform_ = t; }
private:
Correspondances *parent_;
@@ -87,7 +87,7 @@ class Correspondances {
ftl::rgbd::Source *src_;
pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud_;
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud_;
- Eigen::Matrix4f transform_;
+ Eigen::Matrix4d transform_;
bool uptodate_;
std::vector<std::tuple<int,int,int,int>> log_;
cv::Mat src_index_;
diff --git a/components/renderers/cpp/include/ftl/rgbd_display.hpp b/components/renderers/cpp/include/ftl/rgbd_display.hpp
index 2d0c26a365e7bd0f9b3fe7e71bf9d24481334f9f..9c1be76fb5f38a584d3032d50b6ef046f0d0b605 100644
--- a/components/renderers/cpp/include/ftl/rgbd_display.hpp
+++ b/components/renderers/cpp/include/ftl/rgbd_display.hpp
@@ -28,10 +28,10 @@ class Display : public ftl::Configurable {
Source *source_;
std::string name_;
std::vector<std::function<void(int)>> key_handlers_;
- Eigen::Vector3f eye_;
- Eigen::Vector3f centre_;
- Eigen::Vector3f up_;
- Eigen::Vector3f lookPoint_;
+ Eigen::Vector3d eye_;
+ Eigen::Vector3d centre_;
+ Eigen::Vector3d up_;
+ Eigen::Vector3d lookPoint_;
float lerpSpeed_;
bool active_;
MouseAction mouseaction_;
diff --git a/components/renderers/cpp/src/rgbd_display.cpp b/components/renderers/cpp/src/rgbd_display.cpp
index 0623bf38c91116b116b85678e732ce98741f0a23..a0d79f8aeeb42b0a0a1fd281c5f3e9065b43780c 100644
--- a/components/renderers/cpp/src/rgbd_display.cpp
+++ b/components/renderers/cpp/src/rgbd_display.cpp
@@ -64,20 +64,20 @@ void Display::init() {
source_ = nullptr;
cv::namedWindow(name_, cv::WINDOW_KEEPRATIO);
- eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
- centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
- up_ = Eigen::Vector3f(0,1.0f,0);
- lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
+ eye_ = Eigen::Vector3d(0.0, 0.0, 0.0);
+ centre_ = Eigen::Vector3d(0.0, 0.0, -4.0);
+ up_ = Eigen::Vector3d(0,1.0,0);
+ lookPoint_ = Eigen::Vector3d(0.0,0.0,-4.0);
lerpSpeed_ = 0.4f;
// Keyboard camera controls
onKey([this](int key) {
//LOG(INFO) << "Key = " << key;
if (key == 81 || key == 83) {
- Eigen::Quaternion<float> q; q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, up_);
+ Eigen::Quaternion<double> q; q = Eigen::AngleAxis<double>((key == 81) ? 0.01 : -0.01, up_);
eye_ = (q * (eye_ - centre_)) + centre_;
} else if (key == 84 || key == 82) {
- float scalar = (key == 84) ? 0.99f : 1.01f;
+ double scalar = (key == 84) ? 0.99 : 1.01;
eye_ = ((eye_ - centre_) * scalar) + centre_;
}
});
@@ -86,10 +86,10 @@ void Display::init() {
mouseaction_ = [this]( int event, int ux, int uy, int) {
//LOG(INFO) << "Mouse " << ux << "," << uy;
if (event == 1 && source_) { // click
- Eigen::Vector4f camPos = source_->point(ux,uy);
+ Eigen::Vector4d camPos = source_->point(ux,uy);
camPos *= -1.0f;
- Eigen::Vector4f worldPos = source_->getPose() * camPos;
- lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
+ Eigen::Vector4d worldPos = source_->getPose() * camPos;
+ lookPoint_ = Eigen::Vector3d(worldPos[0],worldPos[1],worldPos[2]);
LOG(INFO) << "Depth at click = " << -camPos[2];
}
};
@@ -118,7 +118,7 @@ void Display::update() {
if (!source_) return;
centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
- Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
+ Eigen::Matrix4d viewPose = lookAt<double>(eye_,centre_,up_).inverse();
source_->setPose(viewPose);
Mat rgb, depth;
diff --git a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
index 65d4b5d3f710973871103faab84cd51ffb33d321..cee6e61794db6a7d9cead54d9ffa2d035baee7f7 100644
--- a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
@@ -22,7 +22,7 @@ class Source {
virtual bool grab()=0;
virtual bool isReady() { return false; };
- virtual void setPose(const Eigen::Matrix4f &pose) { };
+ virtual void setPose(const Eigen::Matrix4d &pose) { };
protected:
ftl::rgbd::Source *host_;
diff --git a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp b/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
index 7e055b7cc3e044450a6d4d7bae29d4a1a6a2ca40..9643b1312b59ba068e300bc304a01c3f62d5d955 100644
--- a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
@@ -24,9 +24,9 @@ public:
explicit SnapshotWriter(const std::string &filename);
~SnapshotWriter();
- bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth, const Eigen::Matrix4f &pose, const ftl::rgbd::Camera ¶ms);
+ bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth, const Eigen::Matrix4d &pose, const ftl::rgbd::Camera ¶ms);
bool addMat(const std::string &name, const cv::Mat &mat, const std::string &format="tiff");
- bool addEigenMatrix4f(const std::string &name, const Eigen::Matrix4f &m, const std::string &format="pfm");
+ bool addEigenMatrix4d(const std::string &name, const Eigen::Matrix4d &m, const std::string &format="pfm");
bool addFile(const std::string &name, const std::vector<uchar> &buf);
bool addFile(const std::string &name, const uchar *buf, const size_t len);
@@ -38,7 +38,7 @@ private:
struct SnapshotEntry {
cv::Mat rgb;
cv::Mat depth;
- Eigen::Matrix4f pose;
+ Eigen::Matrix4d pose;
ftl::rgbd::Camera params;
uint status;
SnapshotEntry() : status(1+2+4+8) {};
@@ -49,7 +49,7 @@ public:
explicit SnapshotReader(const std::string &filename);
~SnapshotReader();
- bool getCameraRGBD(const std::string &id, cv::Mat &rgb, cv::Mat &depth, Eigen::Matrix4f &pose, ftl::rgbd::Camera ¶ms);
+ bool getCameraRGBD(const std::string &id, cv::Mat &rgb, cv::Mat &depth, Eigen::Matrix4d &pose, ftl::rgbd::Camera ¶ms);
std::vector<std::string> getIds();
private:
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 89ec80e42869c78d9c86539dc4d34026b144a046..a810fb6f947bf8a80aab7f8205ec8e73d39900b7 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -116,12 +116,12 @@ class Source : public ftl::Configurable {
* cameras this will move the camera, for physical cameras it is set by the
* registration process as it attempts to work out a cameras relative pose.
*/
- virtual void setPose(const Eigen::Matrix4f &pose);
+ virtual void setPose(const Eigen::Matrix4d &pose);
/**
* Get the camera position as a pose matrix.
*/
- const Eigen::Matrix4f &getPose() const;
+ const Eigen::Matrix4d &getPose() const;
/**
* Check what features this source has available.
@@ -131,15 +131,15 @@ class Source : public ftl::Configurable {
/**
* Get a point in camera coordinates at specified pixel location.
*/
- Eigen::Vector4f point(uint x, uint y);
+ Eigen::Vector4d point(uint x, uint y);
/**
* Get a point in camera coordinates at specified pixel location, with a
* given depth value.
*/
- Eigen::Vector4f point(uint x, uint y, float d);
+ Eigen::Vector4d point(uint x, uint y, double d);
- Eigen::Vector2i point(const Eigen::Vector4f &p);
+ Eigen::Vector2i point(const Eigen::Vector4d &p);
/**
* Force the internal implementation to be reconstructed.
@@ -159,7 +159,7 @@ class Source : public ftl::Configurable {
cv::Mat rgb_;
cv::Mat depth_;
Camera params_; // TODO Find better solution
- Eigen::Matrix4f pose_;
+ Eigen::Matrix4d pose_;
ftl::net::Universe *net_;
std::shared_mutex mutex_;
diff --git a/components/rgbd-sources/src/local.cpp b/components/rgbd-sources/src/local.cpp
index a0d6334b357e8c51243fa2922123333092640e23..02bf0d36ef9c1635633d8596c4bc48cf398a3540 100644
--- a/components/rgbd-sources/src/local.cpp
+++ b/components/rgbd-sources/src/local.cpp
@@ -270,10 +270,11 @@ bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
0, 0, cv::INTER_LINEAR);
}
- cv::Ptr<cv::xphoto::WhiteBalancer> wb;
+ // Note: this seems to be too slow on CPU...
+ /*cv::Ptr<cv::xphoto::WhiteBalancer> wb;
wb = cv::xphoto::createSimpleWB();
wb->balanceWhite(l, l);
- wb->balanceWhite(r, r);
+ wb->balanceWhite(r, r);*/
if (flip_v_) {
Mat tl, tr;
diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp
index bd4d5127fad7bd6375226d580f5f01b2441c98ca..674bc24719233954f6a1d32f10bf6a506a9220db 100644
--- a/components/rgbd-sources/src/net.cpp
+++ b/components/rgbd-sources/src/net.cpp
@@ -115,7 +115,7 @@ void NetSource::_recvChunk(int frame, int chunk, bool delta, const vector<unsign
//}
}
-void NetSource::setPose(const Eigen::Matrix4f &pose) {
+void NetSource::setPose(const Eigen::Matrix4d &pose) {
if (!active_) return;
vector<unsigned char> vec((unsigned char*)pose.data(), (unsigned char*)(pose.data()+(pose.size())));
diff --git a/components/rgbd-sources/src/net.hpp b/components/rgbd-sources/src/net.hpp
index 3a986d0647ae465e355dbf887e283f61a43b53b3..ce88d10c399badc35c18cfeb74883e4aaa8afce8 100644
--- a/components/rgbd-sources/src/net.hpp
+++ b/components/rgbd-sources/src/net.hpp
@@ -25,7 +25,7 @@ class NetSource : public detail::Source {
bool grab();
bool isReady();
- void setPose(const Eigen::Matrix4f &pose);
+ void setPose(const Eigen::Matrix4d &pose);
void reset();
diff --git a/components/rgbd-sources/src/snapshot.cpp b/components/rgbd-sources/src/snapshot.cpp
index 51b8a635f1ac906773ccb038f1dfa50e692765da..5599364c6508184c57651d7c46ca54951ac0d4ab 100644
--- a/components/rgbd-sources/src/snapshot.cpp
+++ b/components/rgbd-sources/src/snapshot.cpp
@@ -5,7 +5,7 @@
using namespace ftl::rgbd;
using cv::Mat;
-using Eigen::Matrix4f;
+using Eigen::Matrix4d;
using cv::imencode;
using cv::imdecode;
@@ -13,8 +13,6 @@ using cv::imdecode;
using std::string;
using std::vector;
-using Eigen::Matrix4f;
-
// TODO: move to camera_params
using ftl::rgbd::Camera;
@@ -120,18 +118,18 @@ bool SnapshotWriter::addMat(const string &name, const Mat &mat, const std::strin
return retval;
}
-bool SnapshotWriter::addEigenMatrix4f(const string &name, const Matrix4f &m, const string &format) {
+bool SnapshotWriter::addEigenMatrix4d(const string &name, const Matrix4d &m, const string &format) {
Mat tmp;
cv::eigen2cv(m, tmp);
return addMat(name, tmp, format);
}
bool SnapshotWriter::addCameraRGBD(const string &name, const Mat &rgb, const Mat &depth,
- const Matrix4f &pose, const Camera ¶ms) {
+ const Matrix4d &pose, const Camera ¶ms) {
bool retval = true;
retval &= addMat(name + "-RGB", rgb);
retval &= addMat(name + "-D", depth);
- retval &= addEigenMatrix4f(name + "-POSE", pose);
+ retval &= addEigenMatrix4d(name + "-POSE", pose);
nlohmann::json j;
to_json(j, params);
@@ -223,7 +221,7 @@ bool SnapshotReader::readArchive() {
if (!readEntry(data)) continue;
Mat m_ = cv::imdecode(Mat(data), 0);
if ((m_.rows != 4) || (m_.cols != 4)) continue;
- cv::Matx44f pose_(m_);
+ cv::Matx44d pose_(m_);
cv::cv2eigen(pose_, snapshot.pose);
snapshot.status &= ~(1 << 2);
}
@@ -256,7 +254,7 @@ vector<string> SnapshotReader::getIds() {
}
bool SnapshotReader::getCameraRGBD(const string &id, Mat &rgb, Mat &depth,
- Matrix4f &pose, Camera ¶ms) {
+ Matrix4d &pose, Camera ¶ms) {
if (data_.find(id) == data_.end()) {
LOG(ERROR) << "entry not found: " << id;
return false;
diff --git a/components/rgbd-sources/src/snapshot_source.cpp b/components/rgbd-sources/src/snapshot_source.cpp
index 96c16e4d086df4bf39e7e8d884cc238d929b9167..367fc2861ef84647d6f600a68124e83eb04edf22 100644
--- a/components/rgbd-sources/src/snapshot_source.cpp
+++ b/components/rgbd-sources/src/snapshot_source.cpp
@@ -10,7 +10,7 @@ using ftl::rgbd::detail::SnapshotSource;
using std::string;
SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, SnapshotReader &reader, const string &id) : detail::Source(host) {
- Eigen::Matrix4f pose;
+ Eigen::Matrix4d pose;
reader.getCameraRGBD(id, rgb_, depth_, pose, params_);
setPose(pose);
}
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 8e2864f22b61e5900f95e26d01a9dd32e56c4bd0..b3b9e82b27a54f56bd2f0242d72ad60be6f338e4 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -27,7 +27,7 @@ using ftl::rgbd::detail::NetSource;
using ftl::rgbd::detail::ImageSource;
using ftl::rgbd::capability_t;
-Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4f::Identity()), net_(nullptr) {
+Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(nullptr) {
impl_ = nullptr;
params_ = {0};
reset();
@@ -38,7 +38,7 @@ Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matri
});
}
-Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), pose_(Eigen::Matrix4f::Identity()), net_(net) {
+Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(net) {
impl_ = nullptr;
params_ = {0};
reset();
@@ -147,38 +147,38 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) {
depth = depth_;
}
-Eigen::Vector4f Source::point(uint ux, uint uy) {
+Eigen::Vector4d Source::point(uint ux, uint uy) {
const auto ¶ms = parameters();
- const float x = ((float)ux+(float)params.cx) / (float)params.fx;
- const float y = ((float)uy+(float)params.cy) / (float)params.fy;
+ const double x = ((double)ux+params.cx) / params.fx;
+ const double y = ((double)uy+params.cy) / params.fy;
SHARED_LOCK(mutex_,lk);
- const float depth = depth_.at<float>(uy,ux);
- return Eigen::Vector4f(x*depth,y*depth,depth,1.0);
+ const double depth = depth_.at<float>(uy,ux);
+ return Eigen::Vector4d(x*depth,y*depth,depth,1.0);
}
-Eigen::Vector4f Source::point(uint ux, uint uy, float d) {
+Eigen::Vector4d Source::point(uint ux, uint uy, double d) {
const auto ¶ms = parameters();
- const float x = ((float)ux+(float)params.cx) / (float)params.fx;
- const float y = ((float)uy+(float)params.cy) / (float)params.fy;
- return Eigen::Vector4f(x*d,y*d,d,1.0);
+ const double x = ((double)ux+params.cx) / params.fx;
+ const double y = ((double)uy+params.cy) / params.fy;
+ return Eigen::Vector4d(x*d,y*d,d,1.0);
}
-Eigen::Vector2i Source::point(const Eigen::Vector4f &p) {
+Eigen::Vector2i Source::point(const Eigen::Vector4d &p) {
const auto ¶ms = parameters();
- float x = p[0] / p[2];
- float y = p[1] / p[2];
+ double x = p[0] / p[2];
+ double y = p[1] / p[2];
x *= params.fx;
y *= params.fy;
return Eigen::Vector2i((int)(x - params.cx), (int)(y - params.cy));
}
-void Source::setPose(const Eigen::Matrix4f &pose) {
+void Source::setPose(const Eigen::Matrix4d &pose) {
pose_ = pose;
if (impl_) impl_->setPose(pose);
}
-const Eigen::Matrix4f &Source::getPose() const {
+const Eigen::Matrix4d &Source::getPose() const {
return pose_;
}
diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp
index 34ee61f0afc8ecb8fd87d62b1a0f07b04d0e85cd..45ac6d4962631d96eb14fcaf88f8cfc733799757 100644
--- a/components/rgbd-sources/src/streamer.cpp
+++ b/components/rgbd-sources/src/streamer.cpp
@@ -51,7 +51,7 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
SHARED_LOCK(mutex_,slk);
if (sources_.find(uri) != sources_.end()) {
- Eigen::Matrix4f pose;
+ Eigen::Matrix4d pose;
memcpy(pose.data(), buf.data(), buf.size());
sources_[uri]->src->setPose(pose);
}