Eigen::Vector3d GetCameraCenterWorld() {
GLdouble projmat[16];
GLdouble modelmat[16];
glGetDoublev( GL_PROJECTION_MATRIX, projmat);
glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
Eigen::Matrix4d InvModelMat;
Eigen::Matrix4d InvProjMat;
InvModelMat.setZero();
InvProjMat.setZero();
for(int i=0; i<4; i++){
for(int j=0; j<4; j++){
InvModelMat(i,j)=modelmat[4*i+j];
InvProjMat(i,j)=projmat[4*i+j];
//printf("%lf ", InvModelMat[i][j]);
}
//printf("\n");
}
InvModelMat=(InvModelMat*InvProjMat).transpose();
InvModelMat = (InvModelMat.inverse());
Eigen::Vector4d cc(0, 0, 0, 1);
cc=InvModelMat*cc;
if(cc.norm()!=0) cc=cc/cc[3];
return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}
void BezierBoundarySolver::_IterateCurvatureReduction(BezierBoundaryProblem* pProblem,Eigen::Vector4d& params)
{
double epsilon = 0.0001;
//create a jacobian for the parameters by perturbing them
Eigen::Vector4d Jt; //transpose of the jacobian
BezierBoundaryProblem origProblem = *pProblem;
double maxK = _GetMaximumCurvature(pProblem);
for(int ii = 0; ii < 4 ; ii++){
Eigen::Vector4d epsilonParams = params;
epsilonParams[ii] += epsilon;
_Get5thOrderBezier(pProblem,epsilonParams);
_Sample5thOrderBezier(pProblem);
double kPlus = _GetMaximumCurvature(pProblem);
epsilonParams[ii] -= 2*epsilon;
_Get5thOrderBezier(pProblem,epsilonParams);
_Sample5thOrderBezier(pProblem);
double kMinus = _GetMaximumCurvature(pProblem);
Jt[ii] = (kPlus-kMinus)/(2*epsilon);
}
//now that we have Jt, we can calculate JtJ
Eigen::Matrix4d JtJ = Jt*Jt.transpose();
//thikonov regularization
JtJ += Eigen::Matrix4d::Identity();
Eigen::Vector4d deltaParams = JtJ.inverse() * Jt*maxK;
params -= deltaParams;
_Get5thOrderBezier(pProblem,params);
_Sample5thOrderBezier(pProblem);
//double finalMaxK = _GetMaximumCurvature(pProblem);
//dout("2D Iteration took k from " << maxK << " to " << finalMaxK);
}
void Dist_grad(const Eigen::Matrix<double,Eigen::Dynamic,1> &t,
const std::vector<skeleton::BranchContProjSkel::Ptr> &skel,
double &value,
Eigen::Matrix<double,Eigen::Dynamic,1> &gradient)
{
Eigen::Matrix4d A = Eigen::Matrix4d::Zero();
Eigen::Vector4d b = Eigen::Vector4d::Zero();
value = 0.0;
gradient = Eigen::Matrix<double,Eigen::Dynamic,1>::Zero(t.rows(),1);
std::vector<Eigen::Vector4d> ori(skel.size());
std::vector<Eigen::Vector4d> vec(skel.size());
std::vector<Eigen::Vector4d> orijac(skel.size());
std::vector<Eigen::Vector4d> vecjac(skel.size());
std::vector<Eigen::Matrix4d> A_part_jac(skel.size());
std::vector<Eigen::Vector4d> b_part_jac(skel.size());
for(unsigned int i=0;i<skel.size();i++)
{
Compositor<Application<Eigen::Matrix<double,8,1>,Eigen::Vector3d>,Application<Eigen::Vector3d,double> >
fun(skel[i]->getModel()->getR8Fun(),skel[i]->getCompFun());
Eigen::Matrix<double,8,1> veclin = fun(t(i));
Eigen::Matrix<double,8,1> jaclin = fun.jac(t(i));
ori[i] = veclin.block<4,1>(0,0);
vec[i] = veclin.block<4,1>(4,0).normalized();
orijac[i] = jaclin.block<4,1>(0,0);
vecjac[i] = jaclin.block<4,1>(4,0)*(1.0/veclin.block<4,1>(4,0).norm());
Eigen::Matrix4d A_part = Eigen::Matrix4d::Identity() - vec[i]*vec[i].transpose();
Eigen::Vector4d b_part = A_part*ori[i];
A_part_jac[i] = - vecjac[i]*vec[i].transpose() - vec[i]*vecjac[i].transpose();
b_part_jac[i] = A_part_jac[i] * ori[i] + A_part * orijac[i];
A+=A_part;
b+=b_part;
}
Eigen::Matrix4d A_inv = A.inverse();
Eigen::Vector4d P = A_inv*b;
for(unsigned int i=0;i<skel.size();i++)
{
Eigen::Vector4d P_jac = A_inv * A_part_jac[i] * P + A_inv * b_part_jac[i];
double Dist = (P - ori[i]).squaredNorm() - pow((P - ori[i]).dot(vec[i]),2);
gradient(i) = 2*(P_jac - orijac[i]).dot(P - ori[i])
-2*(P_jac - orijac[i]).dot(vec[i]) * (P - ori[i]).dot(vec[i])
-2*(P - ori[i]).dot(vecjac[i]) * (P - ori[i]).dot(vec[i]);
value += Dist;
}
}
SMC_higher_order(/*systems::ExecutionManager* em*/bool status,
const Eigen::Matrix4d coeff, const Eigen::Vector4d delta,
const Eigen::Matrix4d A, const Eigen::Matrix4d B, const float P,
const float Q,
const std::string& sysName = "Slidingmode_Controller") :
System(sysName), referencejpInput(this), referencejvInput(this), referencejaInput(
this), feedbackjpInput(this), feedbackjvInput(this), M(
this), C(this),
controlOutput(this, &controlOutputValue), STATUS(status), Coeff(
coeff), Delta(delta), A(A), B(B), P(P), Q(Q) {
inv_B = B.inverse();
}
double Keyframe::distance( const Eigen::Matrix4d & transform ) const
{
const Eigen::Matrix4d& poseMat = _pose.transformation();
const Eigen::Matrix4d relativePose = poseMat * transform.inverse();
double dist = relativePose.block<3, 1>( 0, 3 ).norm();
// const Eigen::Matrix3d & poseR = poseMat.block<3, 3>( 0, 0 );
// const Eigen::Matrix3d & kfR = transform.block<3, 3>( 0, 0 );
// Eigen::Matrix3d deltaR = poseR.transpose() * kfR;
// Eigen::Vector3d euler = deltaR.eulerAngles( 0, 1, 2 );
// dist += euler.norm();
return dist;
}
Eigen::Vector3d GetPointEyeToWorld(const Eigen::Ref<const Eigen::Vector3d>& _pt) {
GLdouble modelmat[16];
glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
Eigen::Matrix4d InvModelMat;
InvModelMat.setZero();
for(int i=0; i<4; i++){
for(int j=0; j<4; j++){
InvModelMat(i,j)=modelmat[4*i+j];
}
}
InvModelMat=(InvModelMat.transpose());
InvModelMat=(InvModelMat.inverse());
Eigen::Vector4d cc( _pt(0),_pt(1), _pt(2),1);
cc=InvModelMat*cc;
if(cc.norm()!=0) cc=cc/cc[3];
return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}
virtual void operate() {
tmp_theta_pos = this->feedbackjpInput.getValue();
ThetaInput << tmp_theta_pos[0], tmp_theta_pos[1], tmp_theta_pos[2], tmp_theta_pos[3];
M_tmp = this->M.getValue();
Md_tmp = this->Md.getValue();
tmp_theta_vel = this->feedbackjvInput.getValue();
ThetadotInput << tmp_theta_vel[0], tmp_theta_vel[1], tmp_theta_vel[2], tmp_theta_vel[3];
C_tmp = this->C.getValue();
Cd_tmp = this->Cd.getValue();
// Reference position, velocity and accelerations
qd[0] = this->referencejpInput.getValue()[0];
qd[1] = this->referencejpInput.getValue()[1];
qd[2] = this->referencejpInput.getValue()[2];
qd[3] = this->referencejpInput.getValue()[3];
qdd[0] = this->referencejvInput.getValue()[0];
qdd[1] = this->referencejvInput.getValue()[1];
qdd[2] = this->referencejvInput.getValue()[2];
qdd[3] = this->referencejvInput.getValue()[3];
qddd[0] = this->referencejaInput.getValue()[0];
qddd[1] = this->referencejaInput.getValue()[1];
qddd[2] = this->referencejaInput.getValue()[2];
qddd[3] = this->referencejaInput.getValue()[3];
//
//Position error
e = ThetaInput - qd;
//Velocity error
ed = ThetadotInput - qdd;
//Eta
eta = K / b;
//The error matrix
Xtilde.resize(8, 1);
Xtilde << e, ed;
Md_tmpinverse = Md_tmp.inverse();
M_tmpinverse = M_tmp.inverse();
F = -M_tmpinverse * (C_tmp);
Fd = -Md_tmpinverse * (Cd_tmp);
rho = F - Fd;
// The fbar vector
for (i = 0; i < 4; i = i + 1) {
fbar[i] = rho[i] + eta[i] * c[i] * Xtilde[i]
+ (c[i] + eta[i]) * Xtilde[4 + i] + eta[i] * phi[i];
}
//
Ftilde1 << Xtilde[4], -eta[0] * c[0] * Xtilde[0]
- (c[0] + eta[0]) * Xtilde[4] - eta[0] * phi[0], 0;
Ftilde2 << Xtilde[5], -eta[1] * c[1] * Xtilde[1]
- (c[1] + eta[1]) * Xtilde[5] - eta[1] * phi[1], 0;
Ftilde3 << Xtilde[6], -eta[2] * c[2] * Xtilde[2]
- (c[2] + eta[2]) * Xtilde[6] - eta[2] * phi[2], 0;
Ftilde4 << Xtilde[7], -eta[3] * c[3] * Xtilde[3]
- (c[3] + eta[3]) * Xtilde[7] - eta[3] * phi[3], 0;
//The L and Lbar vectors
for (i = 0; i < 4; i = i + 1) {
L[i] = 0.5
* (Xtilde[i] * Xtilde[i] + Xtilde[4 + i] * Xtilde[4 + i]);
Lbar[i] = L[i] + fbar[i] * fbar[i];
}
// The gradient matrices
DVDX1
<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
* W1[0], (W1[0] * Xtilde[0] + W1[1] * Xtilde[4]
+ W1[2] * phi[0]) * W1[1], (W1[0] * Xtilde[0]
+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * W1[2];
DVDX2
<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
* W2[0], (W2[0] * Xtilde[1] + W2[1] * Xtilde[5]
+ W2[2] * phi[1]) * W2[1], (W2[0] * Xtilde[1]
+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * W2[2];
DVDX3
<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
* W3[0], (W3[0] * Xtilde[2] + W3[1] * Xtilde[6]
+ W3[2] * phi[2]) * W3[1], (W3[0] * Xtilde[2]
+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * W3[2];
DVDX4
<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
* W4[0], (W4[0] * Xtilde[3] + W4[1] * Xtilde[7]
+ W4[2] * phi[3]) * W4[1], (W4[0] * Xtilde[3]
+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * W4[2];
DVDW1
<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
* Xtilde[0] + W1[0], (W1[0] * Xtilde[0]
+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * Xtilde[4] + W1[1], (W1[0]
* Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0]) * phi[0]
+ W1[2];
DVDW2
<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
* Xtilde[1] + W2[0], (W2[0] * Xtilde[1]
+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * Xtilde[5] + W2[1], (W2[0]
* Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1]) * phi[1]
+ W2[2];
DVDW3
<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
* Xtilde[2] + W3[0], (W3[0] * Xtilde[2]
+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * Xtilde[6] + W3[1], (W3[0]
* Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2]) * phi[2]
+ W3[2];
DVDW4
<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
* Xtilde[3] + W4[0], (W4[0] * Xtilde[3]
+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * Xtilde[7] + W4[1], (W4[0]
* Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3]) * phi[3]
+ W4[2];
////
s1 = DVDW1.transpose();
s2 = DVDW2.transpose();
s3 = DVDW3.transpose();
s4 = DVDW4.transpose();
Eigen::Matrix3d tmp_Gtilde;
tmp_Gtilde << Gtilde[0] * Gtilde[0], Gtilde[0] * Gtilde[1], Gtilde[0]
* Gtilde[2], Gtilde[1] * Gtilde[0], Gtilde[1] * Gtilde[1], Gtilde[1]
* Gtilde[2], Gtilde[2] * Gtilde[0], Gtilde[2] * Gtilde[1], Gtilde[2]
* Gtilde[2];
//
r1 = -Lbar[0] + 0.25 * DVDX1.dot(tmp_Gtilde * DVDX1)
- DVDX1.dot(Ftilde1);
r2 = -Lbar[1] + 0.25 * DVDX2.dot(tmp_Gtilde * DVDX2)
- DVDX2.dot(Ftilde2);
r3 = -Lbar[2] + 0.25 * DVDX3.dot(tmp_Gtilde * DVDX3)
- DVDX3.dot(Ftilde3);
r4 = -Lbar[3] + 0.25 * DVDX4.dot(tmp_Gtilde * DVDX4)
- DVDX4.dot(Ftilde4);
W1dot = s1.transpose() / (s1.dot(s1)) * r1;
W2dot = s2.transpose() / (s2.dot(s2)) * r2;
W3dot = s3.transpose() / (s3.dot(s3)) * r3;
W4dot = s4.transpose() / (s4.dot(s4)) * r4;
////
ueq1 = 0.5
* (W1[0] * (W1[2] - W1[1]) * Xtilde[0]
+ W1[1] * (W1[2] - W1[1]) * Xtilde[4]
+ W1[2] * (W1[2] - W1[1]) * phi[0]);
ueq2 = 0.5
* (W2[0] * (W2[2] - W2[1]) * Xtilde[1]
+ W2[1] * (W2[2] - W2[1]) * Xtilde[5]
+ W2[2] * (W2[2] - W2[1]) * phi[1]);
ueq3 = 0.5
* (W3[0] * (W3[2] - W3[1]) * Xtilde[2]
+ W3[1] * (W3[2] - W3[1]) * Xtilde[6]
+ W3[2] * (W3[2] - W3[1]) * phi[2]);
ueq4 = 0.5
* (W4[0] * (W4[2] - W4[1]) * Xtilde[3]
+ W4[1] * (W4[2] - W4[1]) * Xtilde[7]
+ W4[2] * (W4[2] - W4[1]) * phi[3]);
//
// Final torque computations
Ueq << ueq1, ueq2, ueq3, ueq4;
Ud = qddd + Md_tmpinverse * Cd_tmp;
Tau = M_tmp * (Ud + Ueq); // Final torque to system.
//
phidot[0] = -fbar[0] - ueq1;
phidot[1] = -fbar[1] - ueq2;
phidot[2] = -fbar[2] - ueq3;
phidot[3] = -fbar[3] - ueq4;
phi = phi + phidot * del;
W1 = W1 + W1dot * del;
W2 = W2 + W2dot * del;
W3 = W3 + W3dot * del;
W4 = W4 + W4dot * del;
torque_tmp[0] = Tau[0];
torque_tmp[1] = Tau[1];
torque_tmp[2] = Tau[2];
torque_tmp[3] = Tau[3];
// torque_tmp[0] = 0;
// torque_tmp[1] = 0;
// torque_tmp[2] = 0;
// torque_tmp[3] = 0;
optslidecontrolOutputValue->setData(&torque_tmp);
}
bool SurfelsRGBDModel::addNewView(const PCloud &cloud_d, Eigen::Matrix4d T_world){
pcl::KdTreeFLANN< pcl::PointXYZRGB > kdtree;
kdtree.setInputCloud (cloud_d);
std::vector<int> indice(1);
std::vector<float> distance(1);
std::vector<int> covered_pixels(cloud_d->size(),0);
Eigen::Vector4d auxp;
Eigen::Vector4d surfel_normal;
Eigen::Vector4d cloudd_normal;
pcl::PointXYZRGB surfel_pt;
int ptoremove=0;
pcl::PointSurfel remove_aux;
//update surfels
for(unsigned int i=0; i<m_surfels.size()-ptoremove; i++){
auxp << m_surfels[i].x, m_surfels[i].y, m_surfels[i].z, 1.0;
//std::cout << auxp.transpose() << "\n";
auxp = T_world.inverse()*auxp;
surfel_pt.x = auxp(0);
surfel_pt.y = auxp(1);
surfel_pt.z = auxp(2);
//std::cout << surfel_pt.x << " " << surfel_pt.y << " " << surfel_pt.z << "\n\n";
//checks if there is any point in range
if (kdtree.nearestKSearch (surfel_pt, 1, indice, distance) != 1) continue;
//std::cout << "depois tree " << distance[0] << " " << mindist*mindist << " " << mindist <<"\n";
if (distance[0]>mindist*mindist){
if (m_surfels[i].confidence < 2)
{
remove_aux=m_surfels[i];
m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
ptoremove++;
i--;
}
continue;
}
//checks normal angles
surfel_normal << m_surfels[i].normal_x,m_surfels[i].normal_y, m_surfels[i].normal_z, 1.0;
cloudd_normal = computeNormal(indice[0],&kdtree,cloud_d);
surfel_normal =T_world.inverse()*surfel_normal;
normalize(surfel_normal);
float normal_angle = acos(cloudd_normal(0)*surfel_normal(0) + cloudd_normal(1)*surfel_normal(1) + cloudd_normal(2)*surfel_normal(2));
//std::cout << normal_angle*180.0/3.14159265 << " " << update_max_normal_angle << "\n";
if (normal_angle > (update_max_normal_angle*3.14159265/180.0))
{
// Removal check. If a surfel has a different normal and is closer to the camera
// than the new scan, remove it.
if ((surfel_pt.z > cloud_d->points[indice[0]].z) && (m_surfels[i].confidence < 2))
{
remove_aux=m_surfels[i];
m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
ptoremove++;
i--;
}else
covered_pixels[indice[0]] = 1;
continue;
}
//checks distance
// If existing surfel is far from new depth value:
// - If existing one had a worst point of view, and was seen only once, remove it.
// - Otherwise do not include the new one.
if (std::fabs(surfel_pt.z - cloud_d->points[indice[0]].z) > update_max_dist){
if (m_surfels[i].confidence < 2){
remove_aux=m_surfels[i];
m_surfels[i]=m_surfels[m_surfels.size()-1-ptoremove];
m_surfels[m_surfels.size()-1-ptoremove]=remove_aux;
ptoremove++;
i--;
}
else
covered_pixels[indice[0]] = 1;
continue;
}
//update surfel
m_surfels[i].x = (m_surfels[i].x + cloud_d->points[indice[0]].x*T_world(0,0) + cloud_d->points[indice[0]].y*T_world(0,1) + cloud_d->points[indice[0]].z*T_world(0,2) + T_world(0,3))/2;
m_surfels[i].y = (m_surfels[i].y + cloud_d->points[indice[0]].x*T_world(1,0) + cloud_d->points[indice[0]].y*T_world(1,1) + cloud_d->points[indice[0]].z*T_world(1,2) + T_world(1,3))/2;
m_surfels[i].z = (m_surfels[i].z + cloud_d->points[indice[0]].x*T_world(2,0) + cloud_d->points[indice[0]].y*T_world(2,1) + cloud_d->points[indice[0]].z*T_world(2,2) + T_world(2,3))/2;
Eigen::Vector4d cloudd_normal2 = T_world*cloudd_normal;
normalize(cloudd_normal2);
m_surfels[i].normal_x = (m_surfels[i].normal_x + cloudd_normal2(0))/2;
m_surfels[i].normal_y = (m_surfels[i].normal_y + cloudd_normal2(1))/2;
m_surfels[i].normal_z = (m_surfels[i].normal_z + cloudd_normal2(2))/2;
unsigned char rgba_ptr[3];
rgba_ptr[0] = m_surfels[i].rgba >> 24;
rgba_ptr[1] = m_surfels[i].rgba >> 16;
rgba_ptr[2] = m_surfels[i].rgba >> 8;
rgba_ptr[0] = (int)( (int)rgba_ptr[0] + cloud_d->points[indice[0]].r )/2;
rgba_ptr[1] = (int)( (int)rgba_ptr[1] + cloud_d->points[indice[0]].g )/2;
rgba_ptr[2] = (int)( (int)rgba_ptr[2] + cloud_d->points[indice[0]].b )/2;
m_surfels[i].rgba = (rgba_ptr[0] << 24) | (rgba_ptr[1] << 16) | (rgba_ptr[2] << 8);
m_surfels[i].radius = std::min((double)m_surfels[i].radius, (1.0/1.414213562) * cloud_d->points[indice[0]].z / (fx * cloudd_normal(2)));
m_surfels[i].confidence = m_surfels[i].confidence + 1.0;
covered_pixels[indice[0]] = 1;
}
//Add new surfels
// Surfel addition
for (unsigned int i = 0; i < cloud_d->size(); i++)
{
if (isnan(cloud_d->points[i].z) || covered_pixels[i]==1) continue;
pcl::PointSurfel surfel;
surfel.x = cloud_d->points[i].x*T_world(0,0) + cloud_d->points[i].y*T_world(0,1) + cloud_d->points[i].z*T_world(0,2) + T_world(0,3);
surfel.y = cloud_d->points[i].x*T_world(1,0) + cloud_d->points[i].y*T_world(1,1) + cloud_d->points[i].z*T_world(1,2) + T_world(1,3);
surfel.z = cloud_d->points[i].x*T_world(2,0) + cloud_d->points[i].y*T_world(2,1) + cloud_d->points[i].z*T_world(2,2) + T_world(2,3);
cloudd_normal = computeNormal(i,&kdtree,cloud_d);
cloudd_normal = T_world*cloudd_normal;
normalize(cloudd_normal);
surfel.normal_x = cloudd_normal(0);
surfel.normal_y = cloudd_normal(1);
surfel.normal_z = cloudd_normal(2);
surfel.rgba = (cloud_d->points[i].r << 24) | (cloud_d->points[i].g << 16) | (cloud_d->points[i].b << 8);
double camera_normal_z = std::max((float)cloudd_normal(2), 0.3f);
surfel.radius = (2.0/1.414213562) * cloud_d->points[i].z / (fx * camera_normal_z);
surfel.confidence = 1.0;
m_surfels.push_back(surfel);
}
//remove Surfels
for(int k=0;k<ptoremove;k++){
m_surfels.pop_back();
}
}
void GlobalOptimization::optimize()
{
while(true)
{
if(newGPS)
{
newGPS = false;
printf("global optimization\n");
TicToc globalOptimizationTime;
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//options.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//add param
mPoseMap.lock();
int length = localPoseMap.size();
// w^t_i w^q_i
double t_array[length][3];
double q_array[length][4];
map<double, vector<double>>::iterator iter;
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
t_array[i][0] = iter->second[0];
t_array[i][1] = iter->second[1];
t_array[i][2] = iter->second[2];
q_array[i][0] = iter->second[3];
q_array[i][1] = iter->second[4];
q_array[i][2] = iter->second[5];
q_array[i][3] = iter->second[6];
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
}
map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
int i = 0;
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
{
//vio factor
iterVIONext = iterVIO;
iterVIONext++;
if(iterVIONext != localPoseMap.end())
{
Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4],
iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4],
iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
Eigen::Matrix4d iTj = wTi.inverse() * wTj;
Eigen::Quaterniond iQj;
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);
ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
iQj.w(), iQj.x(), iQj.y(), iQj.z(),
0.1, 0.01);
problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);
}
//gps factor
double t = iterVIO->first;
iterGPS = GPSPositionMap.find(t);
if (iterGPS != GPSPositionMap.end())
{
ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],
iterGPS->second[2], iterGPS->second[3]);
//printf("inverse weight %f \n", iterGPS->second[3]);
problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
}
}
//mPoseMap.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
// update global pose
//mPoseMap.lock();
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
iter->second = globalPose;
if(i == length - 1)
{
Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();
Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
double t = iter->first;
WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4],
localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();
WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);
WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4],
globalPose[5], globalPose[6]).toRotationMatrix();
WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
}
}
updateGlobalPath();
//printf("global time %f \n", globalOptimizationTime.toc());
mPoseMap.unlock();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}
static bool colorize(const drc::map_image_t& iDepthMap,
const Eigen::Affine3d& iLocalToCamera,
const bot_core::image_t& iImage,
const BotCamTrans* iCamTrans,
bot_core::image_t& oImage) {
oImage.utime = iDepthMap.utime;
oImage.width = iDepthMap.width;
oImage.height = iDepthMap.height;
oImage.row_stride = 3*iDepthMap.width;
oImage.pixelformat = PIXEL_FORMAT_RGB;
oImage.size = oImage.row_stride * oImage.height;
oImage.nmetadata = 0;
oImage.data.resize(oImage.size);
Eigen::Matrix4d xform;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
xform(i,j) = iDepthMap.transform[i][j];
}
}
xform = iLocalToCamera.matrix()*xform.inverse();
for (int i = 0; i < iDepthMap.height; ++i) {
for (int j = 0; j < iDepthMap.width; ++j) {
double z;
if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_FLOAT32) {
z = ((float*)(&iDepthMap.data[0] + i*iDepthMap.row_bytes))[j];
if (z < -1e10) {
continue;
}
}
else if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_UINT8) {
uint8_t val = iDepthMap.data[i*iDepthMap.row_bytes + j];
if (val == 0) {
continue;
}
z = val;
}
else {
continue;
}
Eigen::Vector4d pt = xform*Eigen::Vector4d(j,i,z,1);
double p[3] = {pt(0)/pt(3),pt(1)/pt(3),pt(2)/pt(3)};
double pix[3];
bot_camtrans_project_point(iCamTrans, p, pix);
if (pix[2] < 0) {
continue;
}
uint8_t r,g,b;
if (!interpolate(pix[0], pix[1], iImage, r, g, b)) {
continue;
}
int outImageIndex = i*oImage.row_stride + 3*j;
oImage.data[outImageIndex+0] = r;
oImage.data[outImageIndex+1] = g;
oImage.data[outImageIndex+2] = b;
}
}
return true;
}
static bool colorize(const drc::map_image_t& iDepthMap,
const bot_core::image_t& iImage,
typename pcl::PointCloud<T>::Ptr& oCloud) {
int w(iDepthMap.width), h(iDepthMap.height);
if ((w != iImage.width) || (h != iImage.height)) {
return false;
}
// TODO: for now, reject compressed depth maps
if (iDepthMap.compression != drc::map_image_t::COMPRESSION_NONE) {
return false;
}
// TODO: for now, only accept rgb3 images
if (iImage.pixelformat != PIXEL_FORMAT_RGB) {
return false;
}
if (oCloud == NULL) {
oCloud.reset(new pcl::PointCloud<T>());
}
oCloud->points.clear();
Eigen::Matrix4d xform;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
xform(i,j) = iDepthMap.transform[i][j];
}
}
xform = xform.inverse();
for (int i = 0; i < h; ++i) {
for (int j = 0; j < w; ++j) {
double z;
if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_FLOAT32) {
z = ((float*)(&iDepthMap.data[0] + i*iDepthMap.row_bytes))[j];
if (z < -1e10) {
continue;
}
}
else if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_UINT8) {
uint8_t val = iDepthMap.data[i*iDepthMap.row_bytes + j];
if (val == 0) {
continue;
}
z = val;
}
else {
continue;
}
Eigen::Vector4d pt = xform*Eigen::Vector4d(j,i,z,1);
T newPoint;
newPoint.x = pt(0)/pt(3);
newPoint.y = pt(1)/pt(3);
newPoint.z = pt(2)/pt(3);
int imageIndex = i*iImage.row_stride + 3*j;
newPoint.r = iImage.data[imageIndex+0];
newPoint.g = iImage.data[imageIndex+1];
newPoint.b = iImage.data[imageIndex+2];
oCloud->points.push_back(newPoint);
}
}
oCloud->width = oCloud->points.size();
oCloud->height = 1;
oCloud->is_dense = false;
return true;
}
/*
* here comes the object cluster point cloud in camera frame of reference
*/
void dataCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud) {
if( !create_map_ )
return;
ROS_INFO("creating map");
object_mutex.lock();
ROS_INFO("dataCallback got the mutex");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudIn = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new pcl::PointCloud<pcl::PointXYZRGB>() );
pcl::fromROSMsg(*point_cloud, *pointCloudIn);
// Eigen::Matrix4d objectTransform = Eigen::Matrix4d::Identity();
// change reference frame of point cloud to point mean and oriented along principal axes
Eigen::Vector4d mean;
Eigen::Vector3d eigenvalues;
Eigen::Matrix3d cov;
Eigen::Matrix3d eigenvectors;
pcl::computeMeanAndCovarianceMatrix( *pointCloudIn, cov, mean );
pcl::eigen33( cov, eigenvectors, eigenvalues );
/*
* Added comment to this misterious sign change:
* Assuming the eigenvectors of the object come in the camera frame of reference
* this means the unit vector along the Z axis (Eigen::Vector3d::UnitZ()) points away from the camera.
* The LAST eigenvector points upwards/downwards. If their dot product is positive
* means the eigenvector points downwards. This sign change would
* normalize it so that it always points downwards
*
*/
// z eigenvector
if( Eigen::Vector3d(eigenvectors.col(0)).dot( Eigen::Vector3d::UnitZ() ) > 0.0 )
eigenvectors.col(0) = (-eigenvectors.col(0)).eval();
Eigen::Matrix4d objectTransform = Eigen::Matrix4d::Identity();
objectTransform.block<3,1>(0,0) = eigenvectors.col(2);
objectTransform.block<3,1>(0,1) = eigenvectors.col(1);
objectTransform.block<3,1>(0,2) = eigenvectors.col(0);
objectTransform.block<3,1>(0,3) = mean.block<3,1>(0,0);
if( objectTransform.block<3,3>(0,0).determinant() < 0 ) {
// x eigenvector (z axis)
objectTransform.block<3,1>(0,0) = -objectTransform.block<3,1>(0,0);
}
/*
* -----------------------------
* set roll and pitch to 0,
* leave only the yaw as a free parameter
*/
//flatten_roll_pitch(objectTransform);
/*
* ---------------------------
*/
eigenvectors.col(2) = objectTransform.block<3,1>(0,0); // x axis
eigenvectors.col(1) = objectTransform.block<3,1>(0,1); // y axis
eigenvectors.col(0) = objectTransform.block<3,1>(0,2); // z axis
if(normalise_transform(eigenvectors,point_cloud)){
// x eigenvector
objectTransform.block<3,1>(0,0) = -objectTransform.block<3,1>(0,0);
// y eigenvector
objectTransform.block<3,1>(0,1) = -objectTransform.block<3,1>(0,1);
}
// transform from camera frame of reference to object's main axes frame of reference
Eigen::Matrix4d objectTransformInv = objectTransform.inverse();
//the MRSmap of the object is stored in the frame of reference of the objects' eigen axis
pcl::PointCloud<pcl::PointXYZRGB>::Ptr objectPointCloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new pcl::PointCloud<pcl::PointXYZRGB>() );
pcl::transformPointCloud( *pointCloudIn, *objectPointCloud, (objectTransformInv).cast<float>() );
objectPointCloud->sensor_origin_ = objectTransformInv.block<4,1>(0,3).cast<float>();
objectPointCloud->sensor_orientation_ = Eigen::Quaternionf( objectTransformInv.block<3,3>(0,0).cast<float>() );
treeNodeAllocator_->reset();
map_ = boost::shared_ptr< MultiResolutionSurfelMap >( new MultiResolutionSurfelMap( max_resolution_, max_radius_, treeNodeAllocator_ ) );
map_->params_.dist_dependency = dist_dep_;
std::vector< int > pointIndices( objectPointCloud->points.size() );
for( unsigned int i = 0; i < pointIndices.size(); i++ ) pointIndices[i] = i;
map_->imageAllocator_ = imageAllocator_;
map_->addPoints( *objectPointCloud, pointIndices );
map_->octree_->root_->establishNeighbors();
map_->evaluateSurfels();
map_->buildShapeTextureFeatures();
map_->save( map_folder_ + "/" + object_name_ + ".map" );
if( init_frame_ != "" ) {
ROS_INFO_STREAM( "Looking up transform to <init_frame>=" << init_frame_ <<" from <point_cloud->header.frame_id>="<<point_cloud->header.frame_id );
try {
tf::StampedTransform tf;
tf_listener_->lookupTransform( init_frame_, point_cloud->header.frame_id, point_cloud->header.stamp, tf );
Eigen::Affine3d init_frame_transform;
tf::transformTFToEigen( tf, init_frame_transform );
//obtains the transform of the object frame to the base_link frame of reference
objectTransform = (init_frame_transform.matrix() * objectTransform).eval();
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}
{
Eigen::Quaterniond q( objectTransform.block<3,3>(0,0) );
std::ofstream initPoseFile( map_folder_ + "/" + object_name_ + ".pose" );
initPoseFile << "# x y z qx qy qz qw" << std::endl;
initPoseFile << objectTransform(0,3) << " " << objectTransform(1,3) << " " << objectTransform(2,3) << " "
<< q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;
initPoseFile << "# init_pose: { position: { x: " << objectTransform(0,3) << ", y: " << objectTransform(1,3) << ", z: " << objectTransform(2,3)
<< " }, orientation: { x: " << q.x() << ", y: " << q.y() << ", z: " << q.z() << ", w: " << q.w() << " } } }" << std::endl;
}
cloudv = pcl::PointCloud< pcl::PointXYZRGB >::Ptr( new pcl::PointCloud< pcl::PointXYZRGB >() );
cloudv->header.frame_id = object_name_;
map_->visualize3DColorDistribution( cloudv, -1, -1, false );
Eigen::Quaterniond q( objectTransform.block<3,3>(0,0) );
object_tf_.setIdentity();
object_tf_.setRotation( tf::Quaternion( q.x(), q.y(), q.z(), q.w() ) );
object_tf_.setOrigin( tf::Vector3( objectTransform(0,3), objectTransform(1,3), objectTransform(2,3) ) );
//the object tf is from the camera frame of reference to the object's
object_tf_.stamp_ = point_cloud->header.stamp;
object_tf_.child_frame_id_ = object_name_;
object_available = true; //the object_tf_ member variable now matches the last requested snapshot
if( init_frame_ == "" ) {
object_tf_.frame_id_ = point_cloud->header.frame_id;
}
else
object_tf_.frame_id_ = init_frame_;
first_publication_time = ros::Time::now();
ROS_INFO_STREAM("first_publication_time="<<first_publication_time<<" do_publish_tf_="<<do_publish_tf_);
if(do_publish_tf_){
//ROS_INFO_STREAM("object_tf_.frame_id_="<<object_tf_.frame_id_);
ROS_INFO_STREAM("> Snapshot map :dataCallback() publishing transform object_tf_.frame_id_ (init_frame_)=" << object_tf_.frame_id_ << " child="<< object_tf_.child_frame_id_<<std::endl);
// std::cout <<"Press enter to continue..."<<std::endl;
// std::cin.get();
tf_broadcaster.sendTransform( object_tf_ );
}
sub_cloud_.shutdown();
create_map_ = false;
responseId_++;
object_available = true;
object_mutex.unlock();
ROS_INFO("dataCallback released the mutex");
}