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;
    }
Example #3
0
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());
  }
Example #5
0
	/**
	 * 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
	}
Example #6
0
/**
 * @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;
}
Example #9
0
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();
        }
      }
    }
Example #11
0
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);
    }
}
Example #12
0
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;

}
Example #14
0
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;
}
Example #15
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;
}
Example #16
0
// 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;
}
Example #20
0
 static Eigen::Matrix<T, N, N> get(const Eigen::Matrix<T, N, N> &x) {
     return x.inverse();
 }
Example #21
0
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;
}
Example #22
0
//// 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;
	
}