bool update_map(rm_localization::UpdateMap::Request &req,
rm_localization::UpdateMap::Response &res) {
boost::mutex::scoped_lock lock(closest_keyframe_update_mutex);
Eigen::Vector3f intrinsics;
memcpy(intrinsics.data(), req.intrinsics.data(), 3 * sizeof(float));
bool update_intrinsics = intrinsics[0] != 0.0f;
if (update_intrinsics) {
ROS_INFO("Updated camera intrinsics");
this->intrinsics = intrinsics;
ROS_INFO_STREAM("New intrinsics" << std::endl << this->intrinsics);
}
for (size_t i = 0; i < req.idx.size(); i++) {
Eigen::Quaternionf orientation;
Eigen::Vector3f position, intrinsics;
memcpy(orientation.coeffs().data(),
req.transform[i].unit_quaternion.data(), 4 * sizeof(float));
memcpy(position.data(), req.transform[i].position.data(),
3 * sizeof(float));
Sophus::SE3f new_pos(orientation, position);
if (req.idx[i] == closest_keyframe_idx) {
//closest_keyframe_update_mutex.lock();
camera_position = new_pos
* keyframes[req.idx[i]]->get_pos().inverse()
* camera_position;
keyframes[req.idx[i]]->get_pos() = new_pos;
//if (update_intrinsics) {
// keyframes[req.idx[i]]->get_intrinsics() = intrinsics;
//}
//closest_keyframe_update_mutex.unlock();
} else {
keyframes[req.idx[i]]->get_pos() = new_pos;
//if (update_intrinsics) {
// keyframes[req.idx[i]]->get_intrinsics() = intrinsics;
//}
}
}
return true;
}
void RGBDOdometry::initICPModel(GPUTexture * predictedVertices,
GPUTexture * predictedNormals,
const float depthCutoff,
const Eigen::Matrix4f & modelPose)
{
cudaArray * textPtr;
cudaGraphicsMapResources(1, &predictedVertices->cudaRes);
cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedVertices->cudaRes, 0, 0);
cudaMemcpyFromArray(vmaps_tmp.ptr(), textPtr, 0, 0, vmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice);
cudaGraphicsUnmapResources(1, &predictedVertices->cudaRes);
cudaGraphicsMapResources(1, &predictedNormals->cudaRes);
cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedNormals->cudaRes, 0, 0);
cudaMemcpyFromArray(nmaps_tmp.ptr(), textPtr, 0, 0, nmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice);
cudaGraphicsUnmapResources(1, &predictedNormals->cudaRes);
copyMaps(vmaps_tmp, nmaps_tmp, vmaps_g_prev_[0], nmaps_g_prev_[0]);
for(int i = 1; i < NUM_PYRS; ++i)
{
resizeVMap(vmaps_g_prev_[i - 1], vmaps_g_prev_[i]);
resizeNMap(nmaps_g_prev_[i - 1], nmaps_g_prev_[i]);
}
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3);
Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1);
mat33 device_Rcam = Rcam;
float3 device_tcam = *reinterpret_cast<float3*>(tcam.data());
for(int i = 0; i < NUM_PYRS; ++i)
{
tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
}
cudaDeviceSynchronize();
}
void display()
{
using namespace Eigen;
using namespace igl;
using namespace std;
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// All smooth points
glEnable( GL_POINT_SMOOTH );
lights();
push_scene();
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_NORMALIZE);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
push_object();
if(trackball_on)
{
// Draw a "laser" line
glLineWidth(3.0);
glDisable(GL_LIGHTING);
glEnable(GL_DEPTH_TEST);
glBegin(GL_LINES);
glColor3f(1,0,0);
glVertex3fv(s.data());
glColor3f(1,0,0);
glVertex3fv(d.data());
glEnd();
// Draw the start and end points used for ray
glPointSize(10.0);
glBegin(GL_POINTS);
glColor3f(1,0,0);
glVertex3fv(s.data());
glColor3f(0,0,1);
glVertex3fv(d.data());
glEnd();
}
// Draw the model
glEnable(GL_LIGHTING);
draw_mesh(V,F,N,C);
// Draw all hits
glBegin(GL_POINTS);
glColor3f(0,0.2,0.2);
for(vector<igl::embree::Hit>::iterator hit = hits.begin();
hit != hits.end();
hit++)
{
const double w0 = (1.0-hit->u-hit->v);
const double w1 = hit->u;
const double w2 = hit->v;
VectorXd hitP =
w0 * V.row(F(hit->id,0)) +
w1 * V.row(F(hit->id,1)) +
w2 * V.row(F(hit->id,2));
glVertex3dv(hitP.data());
}
glEnd();
pop_object();
// Draw a nice floor
glPushMatrix();
glEnable(GL_LIGHTING);
glTranslated(0,-1,0);
draw_floor();
glPopMatrix();
// draw a transparent "projection screen" show model at time of hit (aka
// mouse down)
push_object();
if(trackball_on)
{
glColor4f(0,0,0,1.0);
glPointSize(10.0);
glBegin(GL_POINTS);
glVertex3fv(SW.data());
glVertex3fv(SE.data());
glVertex3fv(NE.data());
glVertex3fv(NW.data());
glEnd();
glDisable(GL_LIGHTING);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, tex_id);
glColor4f(1,1,1,0.7);
glBegin(GL_QUADS);
glTexCoord2d(0,0);
glVertex3fv(SW.data());
glTexCoord2d(1,0);
glVertex3fv(SE.data());
glTexCoord2d(1,1);
glVertex3fv(NE.data());
glTexCoord2d(0,1);
glVertex3fv(NW.data());
glEnd();
glBindTexture(GL_TEXTURE_2D, 0);
glDisable(GL_TEXTURE_2D);
}
pop_object();
pop_scene();
// Draw a faint point over mouse
if(!trackball_on)
{
glDisable(GL_LIGHTING);
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f(1.0,0.3,0.3,0.6);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(0,width,0,height);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glPointSize(20.0);
glBegin(GL_POINTS);
glVertex2fv(win_s.data());
glEnd();
}
report_gl_error();
glutSwapBuffers();
glutPostRedisplay();
}
void RGBDOdometry::getIncrementalTransformation(Eigen::Vector3f & trans,
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & rot,
const bool & rgbOnly,
const float & icpWeight,
const bool & pyramid,
const bool & fastOdom,
const bool & so3)
{
bool icp = !rgbOnly && icpWeight > 0;
bool rgb = rgbOnly || icpWeight < 100;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev = rot;
Eigen::Vector3f tprev = trans;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr = Rprev;
Eigen::Vector3f tcurr = tprev;
if(rgb)
{
for(int i = 0; i < NUM_PYRS; i++)
{
computeDerivativeImages(nextImage[i], nextdIdx[i], nextdIdy[i]);
}
}
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> resultR = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Identity();
if(so3)
{
int pyramidLevel = 2;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> R_lr = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Identity();
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();
K(0, 0) = intr(pyramidLevel).fx;
K(1, 1) = intr(pyramidLevel).fy;
K(0, 2) = intr(pyramidLevel).cx;
K(1, 2) = intr(pyramidLevel).cy;
K(2, 2) = 1;
float lastError = std::numeric_limits<float>::max() / 2;
float lastCount = std::numeric_limits<float>::max() / 2;
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> lastResultR = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Identity();
for(int i = 0; i < 10; i++)
{
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> jtj;
Eigen::Matrix<float, 3, 1> jtr;
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> homography = K * resultR * K.inverse();
mat33 imageBasis;
memcpy(&imageBasis.data[0], homography.cast<float>().eval().data(), sizeof(mat33));
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_inv = K.inverse();
mat33 kinv;
memcpy(&kinv.data[0], K_inv.cast<float>().eval().data(), sizeof(mat33));
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_R_lr = K * resultR;
mat33 krlr;
memcpy(&krlr.data[0], K_R_lr.cast<float>().eval().data(), sizeof(mat33));
float residual[2];
TICK("so3Step");
so3Step(lastNextImage[pyramidLevel],
nextImage[pyramidLevel],
imageBasis,
kinv,
krlr,
sumDataSO3,
outDataSO3,
jtj.data(),
jtr.data(),
&residual[0],
GPUConfig::getInstance().so3StepThreads,
GPUConfig::getInstance().so3StepBlocks);
TOCK("so3Step");
lastSO3Error = sqrt(residual[0]) / residual[1];
lastSO3Count = residual[1];
//Converged
if(lastSO3Error < lastError && lastCount == lastSO3Count)
{
break;
}
else if(lastSO3Error > lastError + 0.001) //Diverging
{
lastSO3Error = lastError;
lastSO3Count = lastCount;
resultR = lastResultR;
break;
}
lastError = lastSO3Error;
lastCount = lastSO3Count;
lastResultR = resultR;
Eigen::Vector3f delta = jtj.ldlt().solve(jtr);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rotUpdate = OdometryProvider::rodrigues(delta.cast<double>());
R_lr = rotUpdate.cast<float>() * R_lr;
for(int x = 0; x < 3; x++)
{
for(int y = 0; y < 3; y++)
{
resultR(x, y) = R_lr(x, y);
}
}
}
}
iterations[0] = fastOdom ? 3 : 10;
iterations[1] = pyramid ? 5 : 0;
iterations[2] = pyramid ? 4 : 0;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev_inv = Rprev.inverse();
mat33 device_Rprev_inv = Rprev_inv;
float3 device_tprev = *reinterpret_cast<float3*>(tprev.data());
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> resultRt = Eigen::Matrix<double, 4, 4, Eigen::RowMajor>::Identity();
if(so3)
{
for(int x = 0; x < 3; x++)
{
for(int y = 0; y < 3; y++)
{
resultRt(x, y) = resultR(x, y);
}
}
}
for(int i = NUM_PYRS - 1; i >= 0; i--)
{
if(rgb)
{
projectToPointCloud(lastDepth[i], pointClouds[i], intr, i);
}
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();
K(0, 0) = intr(i).fx;
K(1, 1) = intr(i).fy;
K(0, 2) = intr(i).cx;
K(1, 2) = intr(i).cy;
K(2, 2) = 1;
lastRGBError = std::numeric_limits<float>::max();
for(int j = 0; j < iterations[i]; j++)
{
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Rt = resultRt.inverse();
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R = Rt.topLeftCorner(3, 3);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> KRK_inv = K * R * K.inverse();
mat33 krkInv;
memcpy(&krkInv.data[0], KRK_inv.cast<float>().eval().data(), sizeof(mat33));
Eigen::Vector3d Kt = Rt.topRightCorner(3, 1);
Kt = K * Kt;
float3 kt = {(float)Kt(0), (float)Kt(1), (float)Kt(2)};
int sigma = 0;
int rgbSize = 0;
if(rgb)
{
TICK("computeRgbResidual");
computeRgbResidual(pow(minimumGradientMagnitudes[i], 2.0) / pow(sobelScale, 2.0),
nextdIdx[i],
nextdIdy[i],
lastDepth[i],
nextDepth[i],
lastImage[i],
nextImage[i],
corresImg[i],
sumResidualRGB,
maxDepthDeltaRGB,
kt,
krkInv,
sigma,
rgbSize,
GPUConfig::getInstance().rgbResThreads,
GPUConfig::getInstance().rgbResBlocks);
TOCK("computeRgbResidual");
}
float sigmaVal = std::sqrt((float)sigma / rgbSize == 0 ? 1 : rgbSize);
float rgbError = std::sqrt(sigma) / (rgbSize == 0 ? 1 : rgbSize);
if(rgbOnly && rgbError > lastRGBError)
{
break;
}
lastRGBError = rgbError;
lastRGBCount = rgbSize;
if(rgbOnly)
{
sigmaVal = -1; //Signals the internal optimisation to weight evenly
}
Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_icp;
Eigen::Matrix<float, 6, 1> b_icp;
mat33 device_Rcurr = Rcurr;
float3 device_tcurr = *reinterpret_cast<float3*>(tcurr.data());
DeviceArray2D<float>& vmap_curr = vmaps_curr_[i];
DeviceArray2D<float>& nmap_curr = nmaps_curr_[i];
DeviceArray2D<float>& vmap_g_prev = vmaps_g_prev_[i];
DeviceArray2D<float>& nmap_g_prev = nmaps_g_prev_[i];
float residual[2];
if(icp)
{
TICK("icpStep");
icpStep(device_Rcurr,
device_tcurr,
vmap_curr,
nmap_curr,
device_Rprev_inv,
device_tprev,
intr(i),
vmap_g_prev,
nmap_g_prev,
distThres_,
angleThres_,
sumDataSE3,
outDataSE3,
A_icp.data(),
b_icp.data(),
&residual[0],
GPUConfig::getInstance().icpStepThreads,
GPUConfig::getInstance().icpStepBlocks);
TOCK("icpStep");
}
lastICPError = sqrt(residual[0]) / residual[1];
lastICPCount = residual[1];
Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_rgbd;
Eigen::Matrix<float, 6, 1> b_rgbd;
if(rgb)
{
TICK("rgbStep");
rgbStep(corresImg[i],
sigmaVal,
pointClouds[i],
intr(i).fx,
intr(i).fy,
nextdIdx[i],
nextdIdy[i],
sobelScale,
sumDataSE3,
outDataSE3,
A_rgbd.data(),
b_rgbd.data(),
GPUConfig::getInstance().rgbStepThreads,
GPUConfig::getInstance().rgbStepBlocks);
TOCK("rgbStep");
}
Eigen::Matrix<double, 6, 1> result;
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_rgbd = A_rgbd.cast<double>();
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_icp = A_icp.cast<double>();
Eigen::Matrix<double, 6, 1> db_rgbd = b_rgbd.cast<double>();
Eigen::Matrix<double, 6, 1> db_icp = b_icp.cast<double>();
if(icp && rgb)
{
double w = icpWeight;
lastA = dA_rgbd + w * w * dA_icp;
lastb = db_rgbd + w * db_icp;
result = lastA.ldlt().solve(lastb);
}
else if(icp)
{
lastA = dA_icp;
lastb = db_icp;
result = lastA.ldlt().solve(lastb);
}
else if(rgb)
{
lastA = dA_rgbd;
lastb = db_rgbd;
result = lastA.ldlt().solve(lastb);
}
else
{
assert(false && "Control shouldn't reach here");
}
Eigen::Isometry3f rgbOdom;
OdometryProvider::computeUpdateSE3(resultRt, result, rgbOdom);
Eigen::Isometry3f currentT;
currentT.setIdentity();
currentT.rotate(Rprev);
currentT.translation() = tprev;
currentT = currentT * rgbOdom.inverse();
tcurr = currentT.translation();
Rcurr = currentT.rotation();
}
}
if(rgb && (tcurr - tprev).norm() > 0.3)
{
Rcurr = Rprev;
tcurr = tprev;
}
if(so3)
{
for(int i = 0; i < NUM_PYRS; i++)
{
std::swap(lastNextImage[i], nextImage[i]);
}
}
trans = tcurr;
rot = Rcurr;
}
void set_uniform_vector(GLuint programID, const char* NAME, const Eigen::Vector3f& vector) {
GLuint matrix_id = glGetUniformLocation(programID, NAME);
glUniform3fv(matrix_id, 1, vector.data());
}
void update_map(bool with_intrinsics = false) {
rm_localization::UpdateMap update_map_msg;
update_map_msg.request.idx.resize(map->frames.size());
update_map_msg.request.transform.resize(map->frames.size());
if (with_intrinsics) {
Eigen::Vector3f intrinsics = map->frames[0]->get_intrinsics();
sensor_msgs::SetCameraInfo s;
s.request.camera_info.width = map->frames[0]->get_i(0).cols;
s.request.camera_info.height = map->frames[0]->get_i(0).rows;
// No distortion
s.request.camera_info.D.resize(5, 0.0);
s.request.camera_info.distortion_model =
sensor_msgs::distortion_models::PLUMB_BOB;
// Simple camera matrix: square pixels (fx = fy), principal point at center
s.request.camera_info.K.assign(0.0);
s.request.camera_info.K[0] = s.request.camera_info.K[4] =
intrinsics[0];
s.request.camera_info.K[2] = intrinsics[1];
s.request.camera_info.K[5] = intrinsics[2];
s.request.camera_info.K[8] = 1.0;
// No separate rectified image plane, so R = I
s.request.camera_info.R.assign(0.0);
s.request.camera_info.R[0] = s.request.camera_info.R[4] =
s.request.camera_info.R[8] = 1.0;
// Then P=K(I|0) = (K|0)
s.request.camera_info.P.assign(0.0);
s.request.camera_info.P[0] = s.request.camera_info.P[5] =
s.request.camera_info.K[0]; // fx, fy
s.request.camera_info.P[2] = s.request.camera_info.K[2]; // cx
s.request.camera_info.P[6] = s.request.camera_info.K[5]; // cy
s.request.camera_info.P[10] = 1.0;
set_camera_info_service.call(s);
memcpy(update_map_msg.request.intrinsics.data(), intrinsics.data(),
3 * sizeof(float));
} else {
update_map_msg.request.intrinsics = { {0,0,0}};
}
for (size_t i = 0; i < map->frames.size(); i++) {
update_map_msg.request.idx[i] = map->idx[i];
Sophus::SE3f position = map->frames[i]->get_pos();
memcpy(update_map_msg.request.transform[i].unit_quaternion.data(),
position.unit_quaternion().coeffs().data(),
4 * sizeof(float));
memcpy(update_map_msg.request.transform[i].position.data(),
position.translation().data(), 3 * sizeof(float));
}
update_map_service.call(update_map_msg);
}