UncertainTransformation::covariance_t UncertainTransformation::UOplus() const
{
Eigen::Matrix<double,6,6> S;
S.setIdentity();
S.topRightCorner<3,3>() = -sm::kinematics::crossMx(_t_a_b_a);
return S.inverse().eval()*_U*S.transpose().inverse().eval();
}
bool operator() (const T* const q_coeffs, const T* const t_coeffs, const T* const s, T* residuals) const
{
T q_ceres[4] = {q_coeffs[3], q_coeffs[0], q_coeffs[1], q_coeffs[2]};
Eigen::Matrix<T,3,3> R;
ceres::QuaternionToRotation(q_ceres, ceres::ColumnMajorAdapter3x3(R.data()));
Eigen::Matrix<T,4,4> H = Eigen::Matrix<T,4,4>::Identity();
H.block(0,0,3,3) = *s * R;
H(0,3) = t_coeffs[0];
H(1,3) = t_coeffs[1];
H(2,3) = t_coeffs[2];
Eigen::Matrix<T,4,4> pred_H2 = H * m_H1.cast<T>() * H.inverse();
Eigen::Matrix<T,4,4> err_H = m_H2.cast<T>().inverse() * pred_H2;
Eigen::Matrix<T,3,3> err_R = err_H.block(0,0,3,3);
T roll, pitch, yaw;
mat2RPY(err_R, roll, pitch, yaw);
residuals[0] = err_H(0,3);
residuals[1] = err_H(1,3);
residuals[2] = err_H(2,3);
residuals[3] = roll;
residuals[4] = pitch;
residuals[5] = yaw;
return true;
}
void GIPCam::SetParams(float fx, float fy, float cx, float cy, uint width, uint height) {
// TODO: NOTE: WE IGNORE THE SKEW TERM IN THE GIVEN K MATRIX
const float scale_x = 240.f; // // width*( 1.f/(MAX_X-MIN_X) ); // width*0.5.
const float scale_y = 240.f; // height*( 1.f/(MAX_Y-MIN_Y) ); // height*0.75*0.5.
const float new_fx = scale_x * fx;
const float new_fy = scale_y * fy;
const int new_cx = (int)(scale_x*cx + 180.f); // (float)width/2;
const int new_cy = (int)(scale_y*cy + 240.f); //(float)height/2;
//m_params.m_height = height;
//m_params.m_width = width;
m_params.m_intrinsic.Set(-new_fx, -new_fy, new_cx, new_cy);
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> intrinsic;
intrinsic.setZero();
intrinsic(0,0) = m_params.m_intrinsic.m_fx;
intrinsic(1,1) = m_params.m_intrinsic.m_fy;
intrinsic(2,2) = 1.f;
intrinsic(0,2) = m_params.m_intrinsic.m_cx;
intrinsic(1,2) = m_params.m_intrinsic.m_cy;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> invIntrinsic = intrinsic.inverse();
m_params.m_invIntrinsic.Set(invIntrinsic(0,0), invIntrinsic(1,1), invIntrinsic(0,2), invIntrinsic(1,2));
}
ErrorTermTransformation::ErrorTermTransformation(aslam::backend::TransformationExpression T, sm::kinematics::Transformation prior, Eigen::Matrix<double,6,6> N, int debug) :
_T(T), _prior(prior), _debug(debug)
{
// Fill in the inverse covariance.
setInvR(N.inverse());
// Tell the super class about the design variables:
aslam::backend::DesignVariable::set_t dv;
_T.getDesignVariables(dv);
setDesignVariablesIterator(dv.begin(), dv.end());
}
/**
* invert the matrix
*/
Matrix<M, N> inversed(void) const {
#ifdef CONFIG_ARCH_ARM
Matrix<M, N> res;
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
#else
Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
(this->arm_mat.pData);
Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
Matrix<M, N> res(MyInverse.data());
return res;
#endif
}
/**
* @brief findIntersection for 2D lines. Can handle pll lines (return nothing)
* @param other
* @return
*/
optional<const C2dImagePointPx> C2dLine::findIntersection(const C2dLine & other) const
{
Eigen::Matrix<double, 2, 2> directions;
directions.block<2,1>(0,0) = direction;
directions.block<2,1>(0,1) = -other.direction;
if(fabs(directions.determinant()) < 1e-8)//Should usually be about 1
return optional<const C2dImagePointPx>();
const C2dImagePointPx diff = other.pointOnLine - pointOnLine;
const TEigen2dPoint lambda_mu = directions.inverse() * diff;
const C2dImagePointPx pointOnLine = point(lambda_mu(0));
if(IS_DEBUG) CHECK(!zero((other.point(lambda_mu(1)) - pointOnLine).squaredNorm()), "findIntersection failed (parallel lines?)");
return optional<const C2dImagePointPx>(pointOnLine);
}
void StateEstimatorKinematic::dare(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &B, Eigen::Matrix<double,6,6> &P,int zDim)
{
Eigen::Matrix<double,6,6> Ainv = A.inverse();
Eigen::Matrix<double,6,6> ABRB;
if (zDim == 6)
{
ABRB = Ainv * B * _R.llt().solve(B.transpose());
}
else {
ABRB = Ainv * B.topLeftCorner(6,zDim) * _R.topLeftCorner(zDim,zDim).llt().solve(B.topLeftCorner(6,zDim).transpose());
}
Eigen::Matrix<double,2*6,2*6> Z;
Z.block(0,0,6,6) = Ainv;
Z.block(0,6,6,6) = ABRB;
Z.block(6,0,6,6) = _Q * Ainv;
Z.block(6,6,6,6) = A.transpose() + _Q * ABRB;
Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*6,2*6> > ces;
ces.compute(Z);
Eigen::Matrix<std::complex<double>,2*6,1> eigVal = ces.eigenvalues();
Eigen::Matrix<std::complex<double>,2*6,2*6> eigVec = ces.eigenvectors();
Eigen::Matrix<std::complex<double>,2*6,6> unstableEigVec;
int ctr = 0;
for (int i = 0; i < 2*6; i++) {
if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
unstableEigVec.col(ctr) = eigVec.col(i);
ctr++;
if (ctr > 6)
break;
}
}
Eigen::Matrix<std::complex<double>,6,6> U21inv = unstableEigVec.block(0,0,6,6).inverse();
Eigen::Matrix<std::complex<double>,6,6> PP = unstableEigVec.block(6,0,6,6) * U21inv;
for (int i = 0; i < 6; i++) {
for (int j = 0; j < 6; j++) {
P(i,j) = PP(i,j).real();
}
}
}
/// Trajectory Planner
Vector6d Isella3Controller::Trajectory(double desired_pos, unsigned long time_in, unsigned long time_fin, int iArmModuleId, int iAxisId)
{
Vector6d b;
Vector6d x;
Eigen::Matrix<double, 6, 6> A;
b << theApp.fGetAngleAxisIs(iArmModuleId,iAxisId), 0, 0, desired_pos, 0, 0;
A << 1,time_in,pow(time_in,2),pow(time_in,3),pow(time_in,4),pow(time_in,5),
0,1,2*time_in,3*pow(time_in,2),4*pow(time_in,3),5*pow(time_in,4),
0,0,2,6*time_in,12*pow(time_in,2),20*pow(time_in,3),
1,time_fin,pow(time_fin,2),pow(time_fin,3),pow(time_fin,4),pow(time_fin,5),
0,1,2*time_fin,3*pow(time_fin,2),4*pow(time_fin,3),5*pow(time_fin,4),
0,0,2,6*time_fin,12*pow(time_fin,2),20*pow(time_fin,3);
x = A.inverse()*b;
ROS_INFO("Trajectory ---> desired_pos = %d, AngleAxisIs = %d, time_in = %d, time_fin = %d", (int)desired_pos, (int)theApp.fGetAngleAxisIs(iArmModuleId,iAxisId), (int)time_in, (int)time_fin);
std::cout << "Matrix A:\n" << A << "\nVector b:\n" << b << "\nVector x:\n" << x << "\nVector b = A*x:\n" << A*x << endl;
return x;
}
bool GimbalCalibResidual::operator()(const T *const tau_s,
const T *const tau_d,
const T *const w1,
const T *const w2,
const T *const Lambda1,
const T *const Lambda2,
T *residual) const {
// clang-format off
// Form the transform from static camera to dynamic camera
const Eigen::Matrix<T, 4, 4> T_ds = this->T_ds(tau_s, tau_d, w1, w2, Lambda1, Lambda2);
// Calculate reprojection error by projecting 3D world point observed in
// dynamic camera to static camera
// -- Transform 3D world point from dynamic to static camera
const Eigen::Matrix<T, 3, 1> P_d{T(this->P_d[0]), T(this->P_d[1]), T(this->P_d[2])};
const Eigen::Matrix<T, 3, 1> P_s_cal = (T_ds.inverse() * P_d.homogeneous()).head(3);
// -- Project 3D world point to image plane
const Eigen::Matrix<T, 3, 3> K_s = this->K(T(this->fx_s), T(this->fy_s), T(this->cx_s), T(this->cy_s));
// const Eigen::Matrix<T, 4, 1> D_s = this->D(T(this->k1_s), T(this->k2_s), T(this->k3_s), T(this->k4_s));
// const Eigen::Matrix<T, 2, 1> Q_s_cal = this->project_pinhole_equi(K_s, D_s, P_s_cal);
const Eigen::Matrix<T, 2, 1> Q_s_cal = this->project_pinhole(K_s, P_s_cal);
// -- Calculate reprojection error
residual[0] = T(this->Q_s[0]) - Q_s_cal(0);
residual[1] = T(this->Q_s[1]) - Q_s_cal(1);
// Calculate reprojection error by projecting 3D world point observed in
// static camera to dynamic camera
// -- Transform 3D world point from static to dynamic camera
const Eigen::Matrix<T, 3, 1> P_s{T(this->P_s[0]), T(this->P_s[1]), T(this->P_s[2])};
const Eigen::Matrix<T, 3, 1> P_d_cal = (T_ds * P_s.homogeneous()).head(3);
// -- Project 3D world point to image plane
const Eigen::Matrix<T, 3, 3> K_d = this->K(T(this->fx_d), T(this->fy_d), T(this->cx_d), T(this->cy_d));
// const Eigen::Matrix<T, 4, 1> D_d = this->D(T(this->k1_d), T(this->k2_d), T(this->k3_d), T(this->k4_d));
// const Eigen::Matrix<T, 2, 1> Q_d_cal = this->project_pinhole_equi(K_d, D_d, P_d_cal);
const Eigen::Matrix<T, 2, 1> Q_d_cal = this->project_pinhole(K_d, P_d_cal);
// -- Calculate reprojection error
residual[2] = T(this->Q_d[0]) - Q_d_cal(0);
residual[3] = T(this->Q_d[1]) - Q_d_cal(1);
return true;
// clang-format on
}
void dare(const Eigen::Matrix<double,xDim,xDim> &A, const Eigen::Matrix<double,xDim,uDim> &B, Eigen::Matrix<double,xDim,xDim> &P)
{
Eigen::Matrix<double,xDim,xDim> Ainv = A.inverse();
Eigen::Matrix<double,xDim,xDim> ABRB = Ainv * B * _R.llt().solve(B.transpose());
Eigen::Matrix<double,2*xDim,2*xDim> Z;
Z.block(0,0,xDim,xDim) = Ainv;
Z.block(0,xDim,xDim,xDim) = ABRB;
Z.block(xDim,0,xDim,xDim) = _Q * Ainv;
Z.block(xDim,xDim,xDim,xDim) = A.transpose() + _Q * ABRB;
Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*xDim,2*xDim> > ces;
ces.compute(Z);
Eigen::Matrix<std::complex<double>,2*xDim,1> eigVal = ces.eigenvalues();
Eigen::Matrix<std::complex<double>,2*xDim,2*xDim> eigVec = ces.eigenvectors();
Eigen::Matrix<std::complex<double>,2*xDim,xDim> unstableEigVec;
int ctr = 0;
for (int i = 0; i < 2*xDim; i++) {
if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
unstableEigVec.col(ctr) = eigVec.col(i);
ctr++;
if (ctr > xDim)
break;
}
}
Eigen::Matrix<std::complex<double>,xDim,xDim> U21inv = unstableEigVec.block(0,0,xDim,xDim).inverse();
Eigen::Matrix<std::complex<double>,xDim,xDim> PP = unstableEigVec.block(xDim,0,xDim,xDim) * U21inv;
for (int i = 0; i < xDim; i++) {
for (int j = 0; j < xDim; j++) {
P(i,j) = PP(i,j).real();
}
}
}
void NuTo::ContinuumElementIGA<TDim>::CalculateNMatrixBMatrixDetJacobian(EvaluateDataContinuum<TDim>& rData,
int rTheIP) const
{
// calculate Jacobian
const auto ipCoords = this->GetIntegrationType().GetLocalIntegrationPointCoordinates(rTheIP);
const Eigen::MatrixXd& derivativeShapeFunctionsGeometryNatural =
this->mInterpolationType->Get(Node::eDof::COORDINATES)
.DerivativeShapeFunctionsNaturalIGA(ipCoords, mKnotIDs);
Eigen::Matrix<double, TDim, TDim> jacobian = this->CalculateJacobian(derivativeShapeFunctionsGeometryNatural,
rData.mNodalValues[Node::eDof::COORDINATES]);
// there are two mappings in IGA: 1) reference element <=> parametric space (knots) 2) parametric space <=> physical
// space
// the B-matrix only the invJac of mapping 2) is needed
rData.mDetJacobian = jacobian.determinant();
for (int i = 0; i < TDim; i++)
rData.mDetJacobian *= 0.5 * (mKnots(i, 1) - mKnots(i, 0));
if (rData.mDetJacobian == 0)
throw Exception(std::string("[") + __PRETTY_FUNCTION__ +
"] Determinant of the Jacobian is zero, no inversion possible.");
Eigen::Matrix<double, TDim, TDim> invJacobian = jacobian.inverse();
// calculate shape functions and their derivatives
for (auto dof : this->mInterpolationType->GetDofs())
{
if (dof == Node::eDof::COORDINATES)
continue;
const InterpolationBase& interpolationType = this->mInterpolationType->Get(dof);
rData.mNIGA[dof] = interpolationType.MatrixNIGA(ipCoords, mKnotIDs);
rData.mB[dof] = this->CalculateMatrixB(
dof, interpolationType.DerivativeShapeFunctionsNaturalIGA(ipCoords, mKnotIDs), invJacobian);
}
}
template<int N> void calculateGaussPdf(Eigen::Matrix<double,Eigen::Dynamic,N> X, Eigen::Matrix<double,1,N> mu, Eigen::Matrix<double,N,N> C, double* result){
Eigen::Matrix<double,N,N> L = C.llt().matrixL().transpose(); // cholesky decomposition
Eigen::Matrix<double,N,N> Linv = L.inverse();
double det = L.diagonal().prod(); //determinant of L is equal to square rooot of determinant of C
double lognormconst = -log(2 * M_PI)*X.cols()/2 - log(fabs(det));
Eigen::Matrix<double,1,N> x = mu;
Eigen::Matrix<double,1,N> tmp = x;
for (int i=0; i<X.rows(); i++){
x.noalias() = X.row(i) - mu;
tmp.noalias() = x*Linv;
double exponent = -0.5 * (tmp.cwiseProduct(tmp)).sum();
result[i] = lognormconst+exponent;
}
/*
Eigen::Matrix<double,Eigen::Dynamic,N> X0 = (X.rowwise() - mu)*Linv;
Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,1> > resultMap(result, X.rows());
resultMap = (X0.rowwise().squaredNorm()).array() * (-0.5) + lognormconst;
*/
fmath::expd_v(result, X.rows());
}
void PositionCommand::control()
{
geometry_msgs::Twist vel_cmd_msg;
geometry_msgs::Pose current_error_msg ;
Eigen::Matrix<double,6,1> current_error;
if (control_)
{
current_error= goal_pose_ - current_pose_;
// if the error is less than epsilon goal is reached
if(current_error.norm() < .1)
{
ROS_INFO("Reached goal!");
}
accum_error_ = accum_error_+ period_*(current_error + previous_error_)/2.0;
Eigen::Matrix<double,6,6> Cp = Kp * current_error.transpose();
Eigen::Matrix<double,6,6> Ci = Ki * accum_error_.transpose();
Eigen::Matrix<double,6,6> Cd = Kd * (current_error - previous_error_).transpose()/period_;
//std::cout << "Cp =" << Cp << std::endl;
//std::cout << "Ci =" << Ci << std::endl;
//std::cout << "Cd =" << Cd << std::endl;
Eigen::Matrix<double,6,6> vel_cmd_current = Cp + Ci + Cd ;
Eigen::Matrix<double,3,1> linear_vel_cmd_l_frame;
linear_vel_cmd_l_frame << vel_cmd_current(0,0),
vel_cmd_current(1,1),
vel_cmd_current(2,2);
Eigen::Matrix<double,3,1> linear_vel_cmd_g_frame = rot_matrix_.inverse() * linear_vel_cmd_l_frame;
if (chirp_enable_)
{
linear_vel_cmd_g_frame(0,0) = linear_vel_cmd_g_frame(0,0) + chirp_command();
}
vel_cmd_msg.linear.x = Sat(linear_vel_cmd_g_frame(0,0),1,-1);
vel_cmd_msg.linear.y = Sat(linear_vel_cmd_g_frame(1,0),1,-1);
vel_cmd_msg.linear.z = Sat(linear_vel_cmd_g_frame(2,0),1,-1);
vel_cmd_msg.angular.z = Sat(vel_cmd_current(5,5),1,-1);
previous_error_ = current_error;
control_ = false;
current_error_pub.publish(current_error_msg);
std::cout << "CE: x =" << current_error(0,0)
<< " y = " << current_error(1,0)
<< " z = " << current_error(2,0)
<< " w = " << current_error(5,0)
<< std::endl;
}
current_error_msg.position.x = current_error(0,0);
current_error_msg.position.y = current_error(1,0);
current_error_msg.position.z = current_error(2,0);
current_error_msg.orientation.x = current_error(0,0);
current_error_msg.orientation.y = current_error(1,0);
current_error_msg.orientation.z = current_error(2,0);
vel_cmd.publish(vel_cmd_msg);
// std::cout << "AE: x =" << accum_error_(0,0)
// << " y = " << accum_error_(1,0)
// << " z = " << accum_error_(2,0)
// << " w = " << accum_error_(5,0)
// << std::endl;
std::cout << "Kp = " << Kp.transpose()<< std::endl;
std::cout << "Ki = " << Ki.transpose()<< std::endl;
std::cout << "Kd = " << Kd.transpose()<< std::endl;
std::cout << "vel: x =" << vel_cmd_msg.linear.x
<< " y = " << vel_cmd_msg.linear.y
<< " z = " << vel_cmd_msg.linear.z
<< " w = " << vel_cmd_msg.angular.z
<< std::endl;
}
unsigned int fast_inclusion(const tetra *t, const point *p) {
auto &a = *t->p[0],
&b = *t->p[1],
&c = *t->p[2],
&d = *t->p[3];
Eigen::Matrix<double, 3, 3> A;
A.col(0) << b.x - a.x, b.y - a.y, b.z - a.z;
A.col(1) << c.x - a.x, c.y - a.y, c.z - a.z;
A.col(2) << d.x - a.x, d.y - a.y, d.z - a.z;
if (verbose == 3)
std::cout << A << std::endl;
Eigen::Matrix<double, 3, 1> x, B(p->x - a.x, p->y - a.y, p->z - a.z);
x = A.inverse() * B;
double sum = 0;
for (unsigned int i = 0; i < 3; ++i) {
if (std::abs(x[i]) < 1e-10)
x[i] = 0;
if (x[i] < 0)
return 0; // outside
else
sum += x[i];
}
if (std::abs(sum - 1) < 1e-10)
sum = 1;//return exact_inclusion(t, p);
if (std::abs(sum) < 1e-10)
sum = 0;
if (sum > 1)
return 0; // outside
if (sum == 0)
return 1; // vertex 0
double u(x[0]), v(x[1]), w(x[2]);
if (u == 1) {
return 2; // vertex 1
}
else if (u > 0) {
if (v > 0) {
if (w > 0) {
if (sum == 1)
return 14; // surface 321
else
return 15; // inside
}
else {
if (sum == 1)
return 6; // edge 21
else
return 7; // surface 012
}
}
else {
if (w > 0) {
if (sum == 1)
return 10; // edge 31
else
return 11; // surface 031
}
else {
return 3; // edge 10
}
}
} else {
if (v == 1)
return 4; // vertex 2
else if (v > 0) {
if (w > 0) {
if (sum == 1)
return 12; // edge 32
else
return 13; // surface 023
}
else {
return 5; // edge 20
}
}
else {
if (w == 1)
return 8; // vertex 3
else
return 9; // edge 30
}
}
return 0;
}
void ICPOdometry::getIncrementalTransformation(Eigen::Vector3f & trans, Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & rot, int threads, int blocks)
{
iterations[0] = 10;
iterations[1] = 5;
iterations[2] = 4;
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;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev_inv = Rprev.inverse();
Mat33 & device_Rprev_inv = device_cast<Mat33>(Rprev_inv);
float3& device_tprev = device_cast<float3>(tprev);
cv::Mat resultRt = cv::Mat::eye(4, 4, CV_64FC1);
for(int i = NUM_PYRS - 1; i >= 0; i--)
{
for(int j = 0; j < iterations[i]; j++)
{
Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_icp;
Eigen::Matrix<float, 6, 1> b_icp;
Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);
float3& device_tcurr = device_cast<float3>(tcurr);
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];
icpStep(device_Rcurr,
device_tcurr,
vmap_curr,
nmap_curr,
device_Rprev_inv,
device_tprev,
intr(i),
vmap_g_prev,
nmap_g_prev,
distThres_,
angleThres_,
sumData,
outData,
A_icp.data(),
b_icp.data(),
&residual[0],
threads,
blocks);
lastICPError = sqrt(residual[0]) / residual[1];
lastICPCount = residual[1];
Eigen::Matrix<double, 6, 1> result;
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_icp = A_icp.cast<double>();
Eigen::Matrix<double, 6, 1> db_icp = b_icp.cast<double>();
lastA = dA_icp;
lastb = db_icp;
result = lastA.ldlt().solve(lastb);
Eigen::Isometry3f incOdom;
OdometryProvider::computeProjectiveMatrix(resultRt, result, incOdom);
Eigen::Isometry3f currentT;
currentT.setIdentity();
currentT.rotate(Rprev);
currentT.translation() = tprev;
currentT = currentT * incOdom.inverse();
tcurr = currentT.translation();
Rcurr = currentT.rotation();
}
}
trans = tcurr;
rot = Rcurr;
}
// Takes 2nd Order tensor in Kelvin representation and returns its
// inverse in Kelvin representation
KVec InvertVector(const KVec& vec)
{
Eigen::Matrix<double, 3, 3> tens = KelvinVectorToTensor(vec);
return TensorToKelvinVector(tens.inverse());
}
inline bool
pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree,
pcl::BivariatePolynomialT<real>& ret) const
{
//MEASURE_FUNCTION_TIME;
unsigned int parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
//cout << PVARN (parameters_size);
//cout << "Searching for the "<<parameters_size<<" parameters for the bivariate polynom of degree "
// << polynomial_degree<<" using "<<samplePoints.size ()<<" points.\n";
if (parameters_size > samplePoints.size ()) // Too many parameters for this number of equations (points)?
{
return false;
// Reduce degree of polynomial
//polynomial_degree = (unsigned int) (0.5f* (sqrtf (8*samplePoints.size ()+1) - 3));
//parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
//cout << "Not enough points, so degree of polynomial was decreased to "<<polynomial_degree
// << " ("<<samplePoints.size ()<<" points => "<<parameters_size<<" parameters)\n";
}
ret.setDegree (polynomial_degree);
//double coeffStuffStartTime=-get_time ();
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
A.setZero();
Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
b.setZero();
real currentX, currentY, currentZ;
real tmpX, tmpY;
real *tmpC = new real[parameters_size];
real* tmpCEndPtr = &tmpC[parameters_size-1];
for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
it!=samplePoints.end (); ++it)
{
currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
//cout << "current point: "<<currentX<<","<<currentY<<" => "<<currentZ<<"\n";
//unsigned int posInC = parameters_size-1;
real* tmpCPtr = tmpCEndPtr;
tmpX = 1.0;
for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
{
tmpY = 1.0;
for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree)
{
* (tmpCPtr--) = tmpX*tmpY;
//cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC--<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
tmpY *= currentY;
}
tmpX *= currentX;
}
real* APtr = &A(0,0);
real* bPtr = &b[0];
real* tmpCPtr1=tmpC;
for (unsigned int i=0; i<parameters_size; ++i)
{
* (bPtr++) += currentZ * *tmpCPtr1;
real* tmpCPtr2=tmpC;
for (unsigned int j=0; j<parameters_size; ++j)
{
* (APtr++) += *tmpCPtr1 * * (tmpCPtr2++);
}
++tmpCPtr1;
}
//A += DMatrix<real>::outProd (tmpC);
//b += currentZ * tmpC;
}
//cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
//<< (coeffStuffStartTime+get_time ())*1000<<"ms using constant memory.\n";
//cout << PVARC (A)<<PVARN (b);
//double coeffStuffStartTime=-get_time ();
//DMatrix<real> A (parameters_size, parameters_size);
//DVector<real> b (parameters_size);
//real currentX, currentY, currentZ;
//unsigned int posInC;
//real tmpX, tmpY;
//DVector<real> tmpC (parameters_size);
//for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
// it!=samplePoints.end (); ++it)
//{
//currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
////cout << "x="<<currentX<<", y="<<currentY<<"\n";
//posInC = parameters_size-1;
//tmpX = 1.0;
//for (unsigned int xDegree=0; xDegree<=polynomial_degree; xDegree++)
//{
//tmpY = 1.0;
//for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; yDegree++)
//{
//tmpC[posInC] = tmpX*tmpY;
////cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
//tmpY *= currentY;
//posInC--;
//}
//tmpX *= currentX;
//}
//A += DMatrix<real>::outProd (tmpC);
//b += currentZ * tmpC;
//}
//cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
//<< (coeffStuffStartTime+get_time ())*1000<<"ms.\n";
Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
//double choleskyStartTime=-get_time ();
//parameters = A.choleskySolve (b);
//cout << "Cholesky took "<< (choleskyStartTime+get_time ())*1000<<"ms.\n";
//double invStartTime=-get_time ();
parameters = A.inverse () * b;
//cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n";
//cout << PVARC (A)<<PVARC (b)<<PVARN (parameters);
real inversionCheckResult = (A*parameters - b).norm ();
if (inversionCheckResult > 1e-5)
{
//cout << "Inversion result: "<< inversionCheckResult<<" for matrix "<<A<<"\n";
return false;
}
for (unsigned int i=0; i<parameters_size; i++)
ret.parameters[i] = parameters[i];
//cout << "Resulting polynomial is "<<ret<<"\n";
//Test of gradient: ret.calculateGradient ();
delete [] tmpC;
return true;
}
void ProbabilisticStereoTriangulator<CAMERA_GEOMETRY_T>::getUncertainty(
size_t keypointIdxA, size_t keypointIdxB,
const Eigen::Vector4d& homogeneousPoint_A,
Eigen::Matrix3d& outPointUOplus_A, bool& outCanBeInitialized) const {
OKVIS_ASSERT_TRUE_DBG(Exception,frameA_&&frameB_,"initialize with frames before use!");
// also get the point in the other coordinate representation
//Eigen::Vector4d& homogeneousPoint_B=_T_BA*homogeneousPoint_A;
Eigen::Vector4d hPA = homogeneousPoint_A;
// calculate point uncertainty by constructing the lhs of the Gauss-Newton equation system.
// note: the transformation T_WA is assumed constant and identity w.l.o.g.
Eigen::Matrix<double, 9, 9> H = H_;
// keypointA_t& kptA = _frameA_ptr->keypoint(keypointIdxA);
// keypointB_t& kptB = _frameB_ptr->keypoint(keypointIdxB);
Eigen::Vector2d kptA, kptB;
frameA_->getKeypoint(camIdA_, keypointIdxA, kptA);
frameB_->getKeypoint(camIdB_, keypointIdxB, kptB);
// assemble the stuff from the reprojection errors
double keypointStdDev;
frameA_->getKeypointSize(camIdA_, keypointIdxA, keypointStdDev);
keypointStdDev = 0.8 * keypointStdDev / 12.0;
Eigen::Matrix2d inverseMeasurementCovariance = Eigen::Matrix2d::Identity()
* (1.0 / (keypointStdDev * keypointStdDev));
::okvis::ceres::ReprojectionError<CAMERA_GEOMETRY_T> reprojectionErrorA(
frameA_->geometryAs<CAMERA_GEOMETRY_T>(camIdA_), 0, kptA,
inverseMeasurementCovariance);
//typename keypointA_t::measurement_t residualA;
Eigen::Matrix<double, 2, 1> residualA;
Eigen::Matrix<double, 2, 4, Eigen::RowMajor> J_hpA;
Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_hpA_min;
double* jacobiansA[3];
jacobiansA[0] = 0; // do not calculate, T_WA is fixed identity transform
jacobiansA[1] = J_hpA.data();
jacobiansA[2] = 0; // fixed extrinsics
double* jacobiansA_min[3];
jacobiansA_min[0] = 0; // do not calculate, T_WA is fixed identity transform
jacobiansA_min[1] = J_hpA_min.data();
jacobiansA_min[2] = 0; // fixed extrinsics
const double* parametersA[3];
//const double* test = _poseA.parameters();
parametersA[0] = poseA_.parameters();
parametersA[1] = hPA.data();
parametersA[2] = extrinsics_.parameters();
reprojectionErrorA.EvaluateWithMinimalJacobians(parametersA, residualA.data(),
jacobiansA, jacobiansA_min);
inverseMeasurementCovariance.setIdentity();
frameB_->getKeypointSize(camIdB_, keypointIdxB, keypointStdDev);
keypointStdDev = 0.8 * keypointStdDev / 12.0;
inverseMeasurementCovariance *= 1.0 / (keypointStdDev * keypointStdDev);
::okvis::ceres::ReprojectionError<CAMERA_GEOMETRY_T> reprojectionErrorB(
frameB_->geometryAs<CAMERA_GEOMETRY_T>(camIdB_), 0, kptB,
inverseMeasurementCovariance);
Eigen::Matrix<double, 2, 1> residualB;
Eigen::Matrix<double, 2, 7, Eigen::RowMajor> J_TB;
Eigen::Matrix<double, 2, 6, Eigen::RowMajor> J_TB_min;
Eigen::Matrix<double, 2, 4, Eigen::RowMajor> J_hpB;
Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_hpB_min;
double* jacobiansB[3];
jacobiansB[0] = J_TB.data();
jacobiansB[1] = J_hpB.data();
jacobiansB[2] = 0; // fixed extrinsics
double* jacobiansB_min[3];
jacobiansB_min[0] = J_TB_min.data();
jacobiansB_min[1] = J_hpB_min.data();
jacobiansB_min[2] = 0; // fixed extrinsics
const double* parametersB[3];
parametersB[0] = poseB_.parameters();
parametersB[1] = hPA.data();
parametersB[2] = extrinsics_.parameters();
reprojectionErrorB.EvaluateWithMinimalJacobians(parametersB, residualB.data(),
jacobiansB, jacobiansB_min);
// evaluate again closer:
hPA.head<3>() = 0.8 * (hPA.head<3>() - T_AB_.r() / 2.0 * hPA[3])
+ T_AB_.r() / 2.0 * hPA[3];
reprojectionErrorB.EvaluateWithMinimalJacobians(parametersB, residualB.data(),
jacobiansB, jacobiansB_min);
if (residualB.transpose() * residualB < 4.0)
outCanBeInitialized = false;
else
outCanBeInitialized = true;
// now add to H:
H.bottomRightCorner<3, 3>() += J_hpA_min.transpose() * J_hpA_min;
H.topLeftCorner<6, 6>() += J_TB_min.transpose() * J_TB_min;
H.topRightCorner<6, 3>() += J_TB_min.transpose() * J_hpB_min;
H.bottomLeftCorner<3, 6>() += J_hpB_min.transpose() * J_TB_min;
H.bottomRightCorner<3, 3>() += J_hpB_min.transpose() * J_hpB_min;
// invert (if invertible) to get covariance:
Eigen::Matrix<double, 9, 9> cov;
if (H.colPivHouseholderQr().rank() < 9) {
outCanBeInitialized = false;
return;
}
cov = H.inverse(); // FIXME: use the QR decomposition for this...
outPointUOplus_A = cov.bottomRightCorner<3, 3>();
}
int runGeneralizedICP(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_pointcloud,
Eigen::Matrix<double, 4, 4> source_pointcloud_pose,
Eigen::Matrix<double, 4, 4> target_pointcloud_pose,
Eigen::Matrix<double, 4, 4> *out_pose,
int current_index,
int before_index)
{
static int is_first_icp_run = 1;
Eigen::Matrix<double, 4, 4> out_transform;
Eigen::Matrix<double, 4, 4> src_transform;
Eigen::Matrix<double, 4, 4> final_transform;
/**************************************/
/********** Initializations ***********/
/**************************************/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_pcl_pointcloud_corrected =
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >(
new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgt_pcl_pointcloud_corrected =
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >(
new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud < pcl::PointXYZRGB > out_pcl_pointcloud;
pcl::PointCloud < pcl::PointXYZRGB > out_pcl_pointcloud_transformed;
/**************************************/
/**************** GICP ****************/
/**************************************/
// if (is_first_icp_run) {
// accumulated_correction_transform =
// Eigen::Matrix<double, 4, 4>().Identity();
// is_first_icp_run = 0;
// }
// source_pointcloud_pose = accumulated_correction_transform
// * source_pointcloud_pose;
// target_pointcloud_pose = accumulated_correction_transform
// * target_pointcloud_pose;
pcl::transformPointCloud(*source_pointcloud, *src_pcl_pointcloud_corrected,
(target_pointcloud_pose.inverse() * source_pointcloud_pose).cast<float>());
pcl::transformPointCloud(*target_pointcloud, *tgt_pcl_pointcloud_corrected,
Eigen::Matrix<float, 4, 4>().Identity());
gicp.setInputCloud(src_pcl_pointcloud_corrected);
gicp.setInputTarget(tgt_pcl_pointcloud_corrected);
gicp.align(out_pcl_pointcloud);
out_transform = //target_pointcloud_pose *
gicp.getFinalTransformation().cast<double>();
// * target_pointcloud_pose.inverse();
//*corrected_pose = final_transform = source_pointcloud_pose;
src_pcl_pointcloud_corrected->clear();
tgt_pcl_pointcloud_corrected->clear();
if (gicp.hasConverged())
{
*out_pose = out_transform;
return 1;
}
return 0;
}
static Eigen::Matrix<T, N, N> get(const Eigen::Matrix<T, N, N> &x) {
return x.inverse();
}
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;
}
//// process single key frame
void EvoRec::processKeyFrame(std::string depthFile, std::string colorFile, const FrameInfo& frameInfo)
{
std::ifstream inDepth(depthFile.c_str(),std::ios::binary);
float* idepth = new float[width*height];
inDepth.read((char*)idepth, sizeof(float)* width * height);
cv::Mat imageRGB;
imageRGB = cv::imread(colorFile.c_str(), CV_LOAD_IMAGE_COLOR);
//imageRGB.convertTo(imageRGB, CV_8UC3);
// filter data and convert from float to unsigned short and pixelrgb
int count = 0;
for (int v = 0; v < height; v++)
{
for (int u = 0; u < width; u++)
{
int idx = u + v * width;
// filter some invalid points
float depth = idepth[idx];
/*
if (depth > depthThres || depth > MAX_DEPTH
|| depth < 0)
{
depthData[idx] = 0;
imageData[idx].r = 0;
imageData[idx].g = 0;
imageData[idx].b = 0;
continue;
}
*/
// convert from float to short
depthData[idx] = (unsigned short)depth;
// convert from float to unsigned char
imageData[idx].b = (unsigned char)imageRGB.at<cv::Vec3b>(v,u)[0];
imageData[idx].g = (unsigned char)imageRGB.at<cv::Vec3b>(v,u)[1];
imageData[idx].r = (unsigned char)imageRGB.at<cv::Vec3b>(v,u)[2];
//OpenCV store pixels as BGR.
}
}
// upload cpu data to gpu memory
depthRaw_.upload(depthData, width*2, height, width);
color_.upload(imageData, width*3, height, width);
// convert from column major (default) to row major
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rotMatrix;
for (int ix = 0; ix < 3; ix++)
{
for (int iy = 0; iy < 3; iy++)
{
rotMatrix(ix, iy) = frameInfo.rotMatUnscaled(ix, iy);
}
}
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rotMatrixInverse = rotMatrix.inverse();
Eigen::Vector3f transVec;
for (int ix = 0; ix < 3; ix++)
transVec(ix) = frameInfo.transVec(ix);
float3 device_volume_size = device_cast<const float3> (tsdfV.getSize());
Mat33& device_Rcam = device_cast<Mat33> (rotMatrix);
float3& device_tcam = device_cast<float3> (transVec);
Mat33& device_Rcam_inv = device_cast<Mat33> (rotMatrixInverse);
// static tsdf volume
int3 emptyVoxel;
emptyVoxel.x = 0;
emptyVoxel.y = 0;
emptyVoxel.z = 0;
// aggregate information to tsdf data
printf("aggregating information to tsdf volume ... \n");
integrateTsdfVolume(depthRaw_,
frameInfo.intr,
device_volume_size,
device_Rcam_inv,
device_tcam,
tsdfV.getTsdfTruncDist(),
tsdfV.data(),
depthRawScaled_,
emptyVoxel,
colorV.data(),
color_);
printf("extracting point cloud ... \n");
// extract and update the reconstructed point cloud
const Eigen::Vector3i volumeSize = tsdfV.getResolution();
cloud_buffer_ = tsdfV.fetchCloud(cloud_device_,
emptyVoxel,
colorV.data(),
0, volumeSize(0),
0, volumeSize(1),
0, volumeSize(2),
emptyVoxel);
/*size_t size= cloud_buffer_.size();
size_t size2= cloud_device_.size();
std::cout << "buffer size: " << size << endl;
std::cout << "device size: " << size2 << endl;*/
//DeviceArray<pcl::PointXYZRGB> newBuffer(cloud_device_.ptr(), width*height);
//std::cout << "new buffer size: " << newBuffer.size () << endl;
printf("downloading to cpu data ... \n");
cloud_buffer_.download(cloudRec->points);
//newBuffer.download(cloudRec->points);
//cloud_device_.download(cloudRec->points);
//std::cout << "rec size: " << cloudRec->points.size () << endl;
cloudRec->width = (int)cloudRec->points.size ();
cloudRec->height = 1;
//std::cout << "rec size after: " << cloudRec->points.size () << endl;
printf("KF processed ... \n ");
inDepth.close();
delete idepth;
}