Пример #1
0
Matrix3f Segment::jacobianMatrix() {
    Matrix3f result;
    result.col(0) = jacobianColumn(getX());
    result.col(1) = jacobianColumn(getY());
    result.col(2) = jacobianColumn(getZ());
    return result;
}
/**************************************************************************************************
 *  Procecure                                                                                     *
 *                                                                                                *
 *  Description: swapColumns                                                                      *
 *  Class      : ApproachMovementSpace                                                            *
 **************************************************************************************************/
Matrix3f ApproachMovementSpace::swapColumns(Matrix3f matrix, uint column1, uint column2)
{
	Vector3f aux        = matrix.col(column1);

	matrix.col(column1) = matrix.col(column2);
	matrix.col(column2) = aux;

	return matrix;
}
Пример #3
0
SceneObject& SceneObject::setRotation(const Vector3f& x, const Vector3f& y, const Vector3f& z) {
    Matrix3f M;
    M.col(0) = x;
    M.col(1) = y;
    M.col(2) = z;
    rotation = Quaternionf(M);
    rotation.normalize();
    return *this;
}
Пример #4
0
void FakeKinect::setWorldFromCam(const Eigen::Affine3f &worldFromCam) {
  Vector3f eye, center, up;
  Matrix3f rot;
  rot = worldFromCam.linear();
  eye = worldFromCam.translation();
  center = rot.col(2);
  up = -rot.col(1);
  m_cam->setViewMatrixAsLookAt(toOSGVector(eye), toOSGVector(center+eye), toOSGVector(up));
}
Пример #5
0
/* evaluate cost function for a given assignment of npormals to axes */
float mmf::OptSO3vMFCF::evalCostFunction(Matrix3f& R)
{
  float c = 0.0f;
  for (uint32_t j=0; j<6; ++j) { 
    if(j%2 ==0){
      c -= this->cld_.xSums().col(j).transpose()*R.col(j/2);
    }else{
      c += this->cld_.xSums().col(j).transpose()*R.col(j/2);
    }
  }
  return c;
}
Пример #6
0
void Camera::setDirection(const Vector3f& newDirection)
{
    // TODO implement it computing the rotation between newDirection and current dir ?
    Vector3f up = this->up();
    
    Matrix3f camAxes;

    camAxes.col(2) = (-newDirection).normalized();
    camAxes.col(0) = up.cross( camAxes.col(2) ).normalized();
    camAxes.col(1) = camAxes.col(2).cross( camAxes.col(0) ).normalized();
    setOrientation(Quaternionf(camAxes));
    
    mViewIsUptodate = false;
}
Пример #7
0
void Camera::lookAt(const Vector3f &center, const Vector3f &eye, Vector3f &up) {
    Vector3f z = eye - center;
    z.normalize();
    up.normalize();
    Vector3f x = up.cross(z);
    x.normalize();
    Vector3f y = z.cross(x);
    Matrix3f M;
    M.col(0) = x;
    M.col(1) = y;
    M.col(2) = z;
    std::cout << M << std::endl;
    rotation = Quaternionf(M);
    position = eye;
}
int main(int, char**)
{
  cout.precision(3);
  Matrix3f m;
m.row(0) << 1, 2, 3;
m.block(1,0,2,2) << 4, 5, 7, 8;
m.col(2).tail(2) << 6, 9;		    
std::cout << m;

  return 0;
}
Пример #9
0
void RealtimeMF_openni::normals_cb(float* d_normals, uint8_t* haveData,
    uint32_t w, uint32_t h)
{
  tLog_.tic(-1); // reset all timers
  int32_t nComp = 0;
  float* d_nComp = this->normalExtract->d_normalsComp(nComp);
  Matrix3f kRwBefore = kRw_;
  tLog_.toctic(0,1);

  optSO3_->updateExternalGpuNormals(d_nComp,nComp,3,0);
  double residual = optSO3_->conjugateGradientCUDA(kRw_,nCGIter_);
  double D_KL = optSO3_->D_KL_axisUnif();
  tLog_.toctic(1,2);

  {
    boost::mutex::scoped_lock updateLock(this->updateModelMutex);
    this->normalsImg_ = this->normalExtract->normalsImg();
    if(z_.rows() != w*h) z_.resize(w*h);
    this->normalExtract->uncompressCpu(optSO3_->z().data(), optSO3_->z().rows(),
        z_.data(), z_.rows());

    mfAxes_ = MatrixXf::Zero(3,6);
    for(uint32_t k=0; k<6; ++k){
      int j = k/2; // which of the rotation columns does this belong to
      float sign = (- float(k%2) +0.5f)*2.0f; // sign of the axis
      mfAxes_.col(k) = sign*kRw_.col(j);
    }
    D_KL_= D_KL;
    residual_ = residual;
    this->update_ = true;
    updateLock.unlock();
  }

  tLog_.toc(2); // total time
  tLog_.logCycle();
  cout<<"delta rotation kRw_ = \n"<<kRwBefore*kRw_.transpose()<<endl;
  cout<<"---------------------------------------------------------------------------"<<endl;
  tLog_.printStats();
  cout<<" residual="<<residual_<<"\t D_KL="<<D_KL_<<endl;
  cout<<"---------------------------------------------------------------------------"<<endl;
  fout_<<D_KL_<<" "<<residual_<<endl; fout_.flush();

////  return kRw_;
//  {
//    boost::mutex::scoped_lock updateLock(this->updateModelMutex);
////    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr nDispPtr =
////      normalExtract->normalsPc();
////    nDisp_ = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new
////        pcl::PointCloud<pcl::PointXYZRGB>(*nDispPtr));
////    this->normalsImg_ = this->normalExtract->normalsImg();
//    this->update_ = true;
//  }
};
Пример #10
0
float mmf::OptSO3ApproxGD::conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N)
{
//  std::cout << "R before" << std::endl;
//  std::cout << R << std::endl;
//  Timer t0;
  N =0;
  float res0 = this->computeAssignment(R,N)/float(N);
  if(this->t_ == 0){
   // init karcher means with columns of the rotation matrix (takes longer!)
    qKarch_ << R.col(0),-R.col(0),R.col(1),-R.col(1),R.col(2),-R.col(2);
//    // init karcher means with rotated version of previous karcher means
//    qKarch_ =  (this->Rprev_*R.transpose())*qKarch_; 
  }
  qKarch_ = karcherMeans(qKarch_, 5.e-5, 10);
//  t0.toctic("----- karcher mean");
  // compute a rotation matrix from the karcher means (not necessary)
//  Matrix3f Rkarch;
//  Rkarch.col(0) =  qKarch_.col(0);
//  Rkarch.col(1) =  qKarch_.col(2) - qKarch_.col(2).dot(qKarch_.col(0))
//    *qKarch_.col(0);
//  Rkarch.col(1) /= Rkarch.col(1).norm();
//  Rkarch.col(2) = Rkarch.col(0).cross(Rkarch.col(1));
#ifndef NDEBUG
  cout<<"R: "<<endl<<R<<endl;
//  cout<<"Rkarch: "<<endl<<Rkarch<<endl<<"det(Rkarch)="<<Rkarch.determinant()<<endl;
  std::cout << "qKarch_" << std::endl;
  std::cout << qKarch_ << std::endl;
#endif
//  t0.tic();
  computeSuffcientStatistics();
//  t0.toctic("----- sufficient statistics");
  return res0; // cost fct value
}
Пример #11
0
void MiniGL::setViewport (float pfovy, float pznear, float pzfar, const Vector3f &peyepoint, const Vector3f &plookat)
{
	fovy = pfovy;
	znear = pznear;
	zfar = pzfar;

	glLoadIdentity ();
	gluLookAt (peyepoint [0], peyepoint [1], peyepoint [2], plookat[0], plookat[1], plookat[2], 0, 1, 0);

	Matrix4f transformation;
	float *lookAtMatrix = transformation.data();
	glGetFloatv (GL_MODELVIEW_MATRIX, &lookAtMatrix[0]);
	
	Matrix3f rot;
	Vector3f scale;

	transformation.transposeInPlace();

	rot.row(0) = Vector3f (transformation(0,0), transformation(0,1), transformation(0,2));
	rot.row(1) = Vector3f (transformation(1,0), transformation(1,1), transformation(1,2));
	rot.row(2) = Vector3f (transformation(2,0), transformation(2,1), transformation(2,2));
	scale[0] = rot.col(0).norm();
	scale[1] = rot.col(1).norm();
	scale[2] = rot.col(2).norm();
	m_translation = Vector3f (transformation(3,0), transformation(3,1), transformation(3,2));

	rot.col(0) = 1.0f/scale[0] * rot.col(0);
	rot.col(1) = 1.0f/scale[1] * rot.col(1);
	rot.col(2) = 1.0f/scale[2] * rot.col(2);

	m_zoom = scale[0];
	m_rotation = Quaternionf(rot);

	glLoadIdentity ();
}
Пример #12
0
    void initTable(ColorCloudPtr cloud) {
        MatrixXf corners = getTableCornersRansac(cloud);

        Vector3f xax = corners.row(1) - corners.row(0);
        xax.normalize();
        Vector3f yax = corners.row(3) - corners.row(0);
        yax.normalize();
        Vector3f zax = xax.cross(yax);

        float zsgn = (zax(2) > 0) ? 1 : -1;
        xax *= - zsgn;
        zax *= - zsgn; // so z axis points up

        m_axes.col(0) = xax;
        m_axes.col(1) = yax;
        m_axes.col(2) = zax;

        MatrixXf rotCorners = corners * m_axes;

        m_mins = rotCorners.colwise().minCoeff();
        m_maxes = rotCorners.colwise().maxCoeff();
        m_mins(2) = rotCorners(0,2) + LocalConfig::zClipLow;
        m_maxes(2) = rotCorners(0,2) + LocalConfig::zClipHigh;



        m_transform.setBasis(btMatrix3x3(
                                 xax(0),yax(0),zax(0),
                                 xax(1),yax(1),zax(1),
                                 xax(2),yax(2),zax(2)));
        m_transform.setOrigin(btVector3(corners(0,0), corners(0,1), corners(0,2)));

        m_poly.points = toROSPoints32(toBulletVectors(corners));



        m_inited = true;

    }
Пример #13
0
/* evaluate cost function for a given assignment of npormals to axes */
float mmf::OptSO3ApproxGD::evalCostFunction(Matrix3f& R)
{
  float c = 0.0f;
  for (uint32_t j=0; j<6; ++j)
  { 
    const float dot = max(-1.0f,min(1.0f,(qKarch_.col(j).transpose() * R.col(j/2))(0)));
    if(j%2 ==0){
      c += Ns_(j) * acos(dot)* acos(dot);
    }else{
      c += Ns_(j) * acos(-dot)* acos(-dot);
    }
  }
  return c;
}
int main()
{
    Matrix3f m;
    m << 1, 2, 3,
         4, 5, 6,
         7, 8, 9;
    std::cout << m << std::endl;

    RowVectorXd vec1(3);
    vec1 << 1, 2, 3;
    std::cout << "vec1 = " << vec1 << std::endl;

    RowVectorXd vec2(4);
    vec2 << 1, 4, 9, 16;;
    std::cout << "vec2 = " << vec2 << std::endl;

    RowVectorXd joined(7);
    joined << vec1, vec2;
    std::cout << "joined = " << joined << std::endl;


    MatrixXf matA(2, 2);
    matA << 1, 2, 3, 4;
    MatrixXf matB(4, 4);
    matB << matA, matA/10, matA/10, matA;
    std::cout << matB << std::endl;

    {
    Matrix3f m;
    m.row(0) << 1, 2, 3;
    m.block(1,0,2,2) << 4, 5, 7, 8;
    m.col(2).tail(2) << 6, 9;                   
    std::cout << m << std::endl;
    }

    return 0;
}
/**************************************************************************************************
 *  Procecure                                                                                     *
 *                                                                                                *
 *  Description: gramSchmidtNormalization                                                         *
 *  Class      : ApproachMovementSpace                                                            *
 **************************************************************************************************/
Matrix3f ApproachMovementSpace::gramSchmidtNormalization3f(Matrix3f matrix)
{
	// cout << endl << "Non-Normalized Matrix: " << endl << matrix << endl;

	uint matrix_size = matrix.diagonalSize();

	for(uint column = 0; column < matrix_size; column += 1)
	{
		// Step 1: Normalize vector
		matrix.col(column) = normalizeVector3f( matrix.col(column) );

		// Step 2: Eliminate the projection of this vector
		for(uint column_proj = column + 1; column_proj < matrix_size; column_proj += 1)
		{
			Vector3f vj = matrix.col(column_proj);

			matrix.col(column_proj) -= vj.dot( matrix.col(column) ) * matrix.col(column);
		}
	}

	// cout << endl << "Normalized Matrix: " << endl << matrix << endl;

	return matrix;
}
Пример #16
0
// Get traversability of a pose
bool TensorMap::isTraversable(const geometry_msgs::Pose& pose, bool v) {
    traversability_timer.start();
    const Vector3f position(poseToVector(pose));
    const CellKey& key(positionToIndex(position));
    const TensorCell& cell((*this)[key]);

    // check it is easily traversable
    if (isEasilyTraversable(key)) {
        traversability_timer.stop();
        return true;
    }
    // if not, check in details

    // getting parameters
    const TraversabilityParams& params = tensor_map_params->traversability;
    const float min_stick_sal = params.min_saliency;
    const float max_orientation_angle = params.max_slope;
    const int stick_sal_threshold = params.max_points_in_free_cell;
    const float min_free_cell_ratio = params.min_free_cell_ratio;
    const int nb_max_points = params.max_points_in_bounding_box;
    const int nb_min_support = params.min_support_points;
    const float length = params.length;
    const float width = params.width;
    const float height = params.height;
    const float l_2 = length/2. + params.inflation;
    const float w_2 = width/2. + params.inflation;
    const float ground_buffer = params.ground_buffer;
    const float buf2 = ground_buffer/2;
    const float h_2 = height/2;
    const float h0_2 = (height + buf2)/2;
    const float h1_2 = (height - buf2)/2;

    // check for enough saliency
    if (cell.stick_sal<min_stick_sal) {
        if (v) {
            cout << "Saliency too low: "<<cell.stick_sal<<"/"<<
                 min_stick_sal<<endl;
        }
        traversability_timer.stop();
        return false;
    }
    // check orientation is correct
    if (cell.angle_to_vertical>max_orientation_angle) {
        if (v) {
            cout << "Angle too steep: "<<cell.angle_to_vertical<<"/"<<
                 max_orientation_angle<<endl;
        }
        traversability_timer.stop();
        return false;
    }
    /*
     * check absence of obstacle
     */
    // align orientation to surface
    Matrix3f alignedAxes;
    alignQuaternion(pose.orientation, cell.normal, alignedAxes);
    Vector3f x = alignedAxes.col(0);
    Vector3f y = alignedAxes.col(1);
    Vector3f z = alignedAxes.col(2);
    // generate bounding box
    Vector3f max_corner = l_2*x.array().abs()+\
                          w_2*y.array().abs()+\
                          h0_2*z.array().abs();
    Vector3f center = position + h_2*z;
    CellKey min_key = positionToIndex(center - max_corner);
    CellKey max_key = positionToIndex(center + max_corner);
    // shifted up to separate support layer FIXME
    center += (buf2/2)*z;
    // look through bounding box for possible obstacles
    float nb_robot_cells = 0.;
    float nb_ok_robot_cells = 0.;
    int nb_points_in_cells = 0;
    int nb_support_points = 0;
    for (int i=min_key.i; i<max_key.i+1; ++i)
        for (int j=min_key.j; j<max_key.j+1; ++j)
            for (int k=min_key.k; k<max_key.k+1; ++k) {
                // check cell is in the robot
                TensorCell c = (*this)[CellKey(i, j, k)];
                Vector3f rel_pos = c.position - center;
                Vector3f rel_pos_aligned = alignedAxes.transpose()*rel_pos;
                float rel_x, rel_y, rel_z;
                rel_x = rel_pos_aligned(0);
                rel_y = rel_pos_aligned(1);
                rel_z = rel_pos_aligned(2);
                if ((fabs(rel_x)>l_2)||(fabs(rel_y)>w_2)) {
                    continue;
                }
                if (fabs(rel_z)>h1_2) {
                    if (fabs(rel_z+h_2)<buf2) {
                        nb_support_points += c.nb_points;
                    }
                } else {
                    // check no obstacle
                    nb_robot_cells += 1;
                    if (c.nb_points<=stick_sal_threshold) {
                        nb_ok_robot_cells += 1;
                    } else if (c.angle_to_vertical <= max_orientation_angle) {
                        nb_ok_robot_cells += 1;
                        nb_points_in_cells += c.nb_points;
                    }
                }
            }
    // exit if not enough free cells
    if ((nb_ok_robot_cells/nb_robot_cells)<min_free_cell_ratio)
    {
        if (v) {
            cout << "not enough free cells: "<<
                 nb_ok_robot_cells<<"/"<<
                 min_free_cell_ratio*nb_robot_cells<<" ("<<
                 nb_robot_cells<<")"<<endl;
        }
        traversability_timer.stop();
        return false;
    }
    if (nb_points_in_cells>nb_max_points) {
        if (v) {
            cout << "too many (ground) points in cells: "<<
                 nb_points_in_cells<<"/"<<nb_max_points<<endl;
        }
        traversability_timer.stop();
        return false;
    }
    if (nb_support_points<nb_min_support) {
        if (v) {
            cout << "too few support points in cells: "<<
                 nb_support_points<<"/"<<nb_min_support<<endl;
        }
        traversability_timer.stop();
        return false;
    }
    // cell seems valid
    if (v) {
        cout << "valid" << endl;
    }
    traversability_timer.stop();
    return true;

}
/**************************************************************************************************
 *  Procecure                                                                                     *
 *                                                                                                *
 *  Description: moveHandRelativeToObject                                                         *
 *  Class      : ApproachMovementSpace                                                            *
 **************************************************************************************************/
Matrix4f ApproachMovementSpace::moveHandRelativeToObject(float theta, float phi, float rho, bool update)
{
	RobotPtr     hand = eef  -> getRobot();

	// Don't let the hand collide with the object
	if (rho < 10) rho = 10;

	// Calculate new XYZ coordinates for the hand
	Vector3f new_pos     = object -> getGlobalPose().topLeftCorner (3,3) * calculateCarthesianCoordinates(theta, phi, rho);
	         new_pos    += object -> getGlobalPose().topRightCorner(3,1);

	Matrix4f new_pose    = hand -> getGlobalPose();
	Matrix3f orientation;

	// Calculate First Vector of the orientation Matrix
		// It is colinear with the distance vector between object and hand
	orientation.block(0,0,3,1) = object -> getGlobalPose().topRightCorner(3,1) - new_pos;

	// Calculate the Remainder two orientation vectors
	Matrix3f inertia  = object -> getGlobalPose().topLeftCorner(3,3);
		     inertia *= object -> getInertiaMatrix();

	// Select two vectors from the objects inertia matrix
	     uint vectors_left  = 2;
	for (uint column        = 0; column < 3; column += 1)
	{
		Vector3f v1 = orientation.block(0,0,3,1);

		if( v1.dot( inertia.col(column) ) != 1)
		{
			orientation.block(0,vectors_left,3,1) = inertia.col(column);

			vectors_left -= 1;

			if (vectors_left == 0) break;
		}
	}

	// Check if two new vectors were included
	if ( vectors_left != 0)
	{
		cout << endl << "[Error] Function: moveHandRelativeToObject   |   The two vectors from the object's inertia matrix were not selected.";

		exit(-1);
	}

	// Make the orientation matrix orthonormalized
	orientation  = gramSchmidtNormalization3f(orientation);
	orientation *= offset.topLeftCorner(3,3);

	// Check if the new orientation is mirrored or not
		// If so, correct it
	if ( ( orientation.determinant() - (-1) <  0.01) &&
		 ( orientation.determinant() - (-1) > -0.01) )
	{
		orientation.col(1) *= -1;
	}

	// Update new hand pose
	new_pose.topLeftCorner (3,3) = orientation;
	new_pose.topRightCorner(3,1) = new_pos - orientation * offset.topRightCorner(3,1);

	// Set updated pose
	if (update) hand -> setGlobalPose(new_pose);

	return new_pose;
}
Пример #18
0
// Compute dense voting
TensorCell& TensorMap::computeDenseVoting(const CellKey& key) {
    // timing
    dense_voting_timer.start();

    // getting parameters
    const DenseVotingParams& params = tensor_map_params->dense_voting;
    const int maxNbPts = params.max_knn; // max number of neighbors
    const float sigma = params.sigma; // tensor voting parameter
    const float max_dist = params.max_dist; // maximum radius
    const Vector3f& cell_dims = tensor_map_params->cell_dimensions;
    //const float cell2 = cell_dims.squaredNorm()/4;
    const float obs_dir_offset = params.obs_dir_offset;
    const float absolute_saliency_threshold = params.absolute_saliency_threshold;

    // position of the point in space
    const Vector3f position(indexToPosition(key));
    // data in the point cloud
    auto eigen_vectors = sparse_map.getDescriptorViewByName("eigen_vectors");
    auto eigen_values = sparse_map.getDescriptorViewByName("eigen_values");
    auto obsDirection = sparse_map.getDescriptorViewByName\
                        ("observationDirections");
    // tensor to vote into
    Matrix3f tensor = Matrix3f::Identity();
    // getting observation direction from closest point to realign normal
    float min_dist2 = numeric_limits<float>::infinity();
    int closest_index = -1;
    // nearest neighbor search of points for voters

    VectorXi indices(maxNbPts);
    VectorXf dists2(maxNbPts);
    kdTree->knn(position, indices, dists2, maxNbPts, 0, NNSearchF::SORT_RESULTS,
                max_dist);
    // going through all neighboring points of the center of the cell
    int nb_points = 0;
    int i;
    for (i = 0; i<maxNbPts; ++i) {
        if (dists2(i) == numeric_limits<float>::infinity()) { // no match anymore
            break;
        }
        if (dists2(i) == 0.) { // null vote
            ++nb_points;
            continue;
        }
        int index = indices(i);
        // closest point for the point of view
        if (dists2(i)<min_dist2) {
            min_dist2 = dists2(i);
            closest_index = index;
        }
        // counting the number of points in the cell
        const Vector3f voter = sparse_map.features.col(index).head(3);
        if (((voter-position).array().abs() <= (cell_dims/2).array()).all()) {
            ++nb_points;
        }
        /*
         * Full tensor voting
         */
        const Vector3f v = position - voter;
        Vector3f v_hat;
        if (v.isZero()) {
            v_hat = v;
        } else {
            v_hat = v.normalized();
        }
        // saliencies
        float saliencies[3];
        saliencies[0] = eigen_values(0, index) - eigen_values(1, index);
        saliencies[1] = eigen_values(1, index) - eigen_values(2, index);
        saliencies[2] = eigen_values(2, index);
        MatrixXf e_vectors(eigen_vectors.col(index));
        e_vectors.resize(3, 3);
        // normal spaces
        Matrix3f normal_spaces[3];
        normal_spaces[0] = e_vectors.col(0) * (e_vectors.col(0).transpose());
        normal_spaces[1] = normal_spaces[0] + e_vectors.col(1) * (e_vectors.col(1).transpose());
        normal_spaces[2] = normal_spaces[1] + e_vectors.col(2) * (e_vectors.col(2).transpose());
        // accumulator for all votes of this voter
        Matrix3f A = Matrix3f::Zero();
        // loop for all 3 votes
        for (int d=0; d<3; ++d) {
            // normal vector
            const Vector3f v_n = normal_spaces[d] * v;
            Vector3f v_n_hat;
            if (v_n.isZero()) {
                v_n_hat = v_n;
            } else {
                v_n_hat = v_n.normalized();
            }
            // tangent vector
            const Vector3f v_t = v - v_n;
            Vector3f v_t_hat;
            if (v_t.isZero()) {
                v_t_hat = v_t;
            } else {
                v_t_hat = v_t.normalized();
            }
            // normal for votee
            const float cos_theta = v_hat.dot(v_t_hat);
            const float sin_theta = v_hat.dot(v_n_hat);
            const float cos_2theta = 2*pow(cos_theta, 2) - 1;
            const float sin_2theta = 2*cos_theta*sin_theta;
            const Vector3f v_c_hat = cos_2theta * v_n_hat - sin_2theta * v_t_hat;
            // weight functions
            // using n=4 as in King FIXME
            const float w_angle = pow(cos_theta, 2*4);
            const float z = v.norm()/sigma;
            const float w_dist = pow(z,2)*(pow((z-3),4))/16;
            // vote for this dimension
            const Matrix3f A_d = w_dist*w_angle*v_c_hat*v_c_hat.transpose() +
                                 w_dist*(normal_spaces[d] - v_n_hat*v_n_hat.transpose());
            A += saliencies[d] * A_d;
        }
        // standard voting FIXME
        //tensor += A;
        // normalized voting FIXME
        tensor += A / (eigen_values(0, index) + absolute_saliency_threshold);
        //
        /* // old vote
        // actual vote
        Vector3f e_v = position - voter;
        const float r = sqrt(dists2(i));
        e_v /= r;
        const float stick_sal = eigen_values(0, index) - eigen_values(1, index);
        MatrixXf e_vectors(eigen_vectors.col(index));
        e_vectors.resize(3, 3);
        const Vector3f v_n = e_vectors.col(0);
        const Vector3f v_t = v_n.cross(e_v.cross(v_n));
        const float cos_theta = e_v.dot(v_t);
        const float theta = atan2(e_v.dot(v_n), cos_theta);
        // Decoupled weighting profile for angle and distance
        // Angle based weighting
        const float w_angle = pow(cos_theta,4);
        // Distance based weighting
        const float z = r/sigma;
        const float w_dist = pow(z,2)*(pow((z-3),2))/16;
        // stick vote
        // could compute sin and cos based on cos_theta and sin_theta FIXME
        const Vector3f v_c = v_n*cos(2*theta) - v_t*sin(2*theta);
        const float w = w_angle*w_dist;
        tensor += stick_sal*w*v_c*v_c.transpose();
        //tensor += stick_sal*w*v_c*v_c.transpose()/(eigen_values(0, index) + absolute_saliency_threshold);
        */
    }
    // analyse resulting tensor
    const EigenSolver<Matrix3f> solver(tensor);
    Vector3f eigenVals = solver.eigenvalues().real();
    Matrix3f eigenVectors = solver.eigenvectors().real();
    sortTensor(eigenVals, eigenVectors);
    float stick_sal = eigenVals(0)-eigenVals(1);
    float s = eigenVals(0)-eigenVals(1);
    //float p = eigenVals(1)-eigenVals(2);
    //float b = eigenVals(2);
    float Z = eigenVals(0);

    //cout << "Saliency distribution: (" << s/Z << ", " << p/Z << ", " << b/Z << "); abs_sal=" << Z << "; p_plane=" << s/(Z+absolute_saliency_threshold) << " (" << absolute_saliency_threshold << ")" << endl;
    Vector3f normal;
    if (closest_index == -1) {
        // no point at all
        normal = {0., 0., 0.};
        stick_sal = 0.;
    } else {
        // flip normal according to closest point
        const Vector3f obsDir = obsDirection.col(closest_index)+
                                Vector3f(0, 0, obs_dir_offset);
        const Vector3f v_n = eigenVectors.col(0);
        if (obsDir.dot(v_n)<0.) {
            normal = -v_n;
        } else {
            normal = v_n;
        }
    }

    dense_map.insert({key, TensorCell(position, stick_sal, normal, nb_points,
                                      UNKNOWN, s/(Z+absolute_saliency_threshold))
                     });
    // stopping timer
    dense_voting_timer.stop();

    return dense_map.at(key);
}
Пример #19
0
// Get faster traversability
bool TensorMap::isEasilyTraversable(const CellKey& key) {
    TensorCell& cell((*this)[key]);
    // check cache
    if (cell.traversability!=UNKNOWN) {
        return (cell.traversability==OK);
    }
    // compute it
    const bool v = false;

    // getting parameters
    const TraversabilityParams& params = tensor_map_params->traversability;
    const float min_stick_sal = params.min_saliency;
    const float max_orientation_angle = params.max_slope;
    const int stick_sal_threshold = params.max_points_in_free_cell;
    const float min_free_cell_ratio = params.min_free_cell_ratio;
    const int nb_max_points = params.max_points_in_bounding_box;
    const int nb_min_support = params.min_support_points;
    const float diameter = params.diameter;
    const float d_2 = diameter/2. + params.inflation;
    const float r2 = d_2*d_2;
    const float height = params.height;
    const float ground_buffer = params.ground_buffer;
    const float buf2 = ground_buffer/2;
    const float h_2 = height/2;
    const float h0_2 = (height + buf2)/2;
    const float h1_2 = (height - buf2)/2;

    // check for enough saliency
    if (cell.stick_sal<min_stick_sal) {
        if (v) {
            cout << "Saliency too low: "<<cell.stick_sal<<"/"<<
                 min_stick_sal<<endl;
        }
        cell.traversability = NOT_SALIENCY;
        return false;
    }
    // check orientation is correct
    if (cell.angle_to_vertical>max_orientation_angle) {
        if (v) {
            cout << "Angle too steep: "<<cell.angle_to_vertical<<"/"<<
                 max_orientation_angle<<endl;
        }
        cell.traversability = NOT_ANGLE;
        return false;
    }
    /*
     * check absence of obstacle
     */
    // align orientation to surface
    Matrix3f alignedAxes;
    geometry_msgs::Quaternion quat;
    quat.x = quat.y = quat.z = 0;
    quat.w = 1;
    alignQuaternion(quat, cell.normal, alignedAxes);
    Vector3f x = alignedAxes.col(0);
    Vector3f y = alignedAxes.col(1);
    Vector3f z = alignedAxes.col(2);
    // generate bounding box
    Vector3f max_corner = d_2*x.array().abs()+\
                          d_2*y.array().abs()+\
                          h0_2*z.array().abs();
    Vector3f center = cell.position + h_2*z;
    CellKey min_key = positionToIndex(center - max_corner);
    CellKey max_key = positionToIndex(center + max_corner);
    // shifted up to separate support layer FIXME
    center += buf2/2*z;
    // look through bounding box for possible obstacles
    float nb_robot_cells = 0.;
    float nb_ok_robot_cells = 0.;
    int nb_points_in_cells = 0;
    int nb_support_points = 0;
    for (int i=min_key.i; i<max_key.i+1; ++i)
        for (int j=min_key.j; j<max_key.j+1; ++j)
            for (int k=min_key.k; k<max_key.k+1; ++k) {
                // check cell is in the robot
                TensorCell c = (*this)[CellKey(i, j, k)];
                Vector3f rel_pos = c.position - center;
                Vector3f rel_pos_aligned = alignedAxes.transpose()*rel_pos;
                float rel_x, rel_y, rel_z;
                rel_x = rel_pos_aligned(0);
                rel_y = rel_pos_aligned(1);
                rel_z = rel_pos_aligned(2);
                if (pow(rel_x, 2)+pow(rel_y, 2)>r2) {
                    continue;
                }
                if (fabs(rel_z)>h1_2) {
                    if (fabs(rel_z+h_2)<buf2) {
                        nb_support_points += c.nb_points;
                    }
                } else {
                    // check no obstacle
                    nb_robot_cells += 1;
                    if (c.nb_points<=stick_sal_threshold) {
                        nb_ok_robot_cells += 1;
                    } else if (c.angle_to_vertical <= max_orientation_angle) {
                        nb_ok_robot_cells += 1;
                        nb_points_in_cells += c.nb_points;
                    }
                }
            }
    // exit if not enough free cells
    if ((nb_ok_robot_cells/nb_robot_cells)<min_free_cell_ratio)
    {
        if (v) {
            cout << "not enough free cells: "<<
                 nb_ok_robot_cells<<"/"<<
                 min_free_cell_ratio*nb_robot_cells<<" ("<<
                 nb_robot_cells<<")"<<endl;
        }
        cell.traversability = NOT_OBSTACLE;
        return false;
    }
    if (nb_points_in_cells>nb_max_points) {
        if (v) {
            cout << "too many (ground) points in cells: "<<
                 nb_points_in_cells<<"/"<<nb_max_points<<endl;
        }
        cell.traversability = NOT_IN_GROUND;
        return false;
    }
    if (nb_support_points<nb_min_support) {
        if (v) {
            cout << "too few support points in cells: "<<
                 nb_support_points<<"/"<<nb_min_support<<endl;
        }
        cell.traversability = NOT_IN_AIR;
        return false;
    }
    // cell seems valid
    if (v) {
        cout << "valid" << endl;
    }
    cell.traversability = OK;
    return true;
}
Пример #20
0
int main(int argc, char** argv) {
    MatrixXf m(3,3);
    m << 1,2,3,4,5,6,7,8,9;
    Vector3f v =  m.col(0);
    Vector3f v2 =  m.col(1);
    Vector3f v3 =  m.col(2);

    cout<<"Mat = "<<m<<endl;
    cout<<"vector = " << v<<endl;

    MatrixXf n = (m.rowwise() -= v);
    cout<<"Mat2 = "<<n<<endl;

    Matrix3f r;
    r.col(0)  = v;
    r.col(1)  = v2;
    r.col(2)  = v3;
    cout<<"Mat3 = "<<r<<endl;
    r *= 2;
    cout<<"Mat3 = "<<r<<endl;
    cout<<"Mat3 = "<<m<<endl;


    float arr[] = {1,2,3};
    vector<float> w(arr, arr + sizeof(arr) / sizeof(float));
    for(int i = 0; i < w.size(); i+=1)
        cout<<w[i]<<"\t";
    cout<<endl<<"---------------"<<endl;

    /** Inverting a singular matrix. **/
    m << 1,0,0,1,0,0,1,0,0;
    cout<<" Should segfault/ get garbage: "<<m.determinant()<<endl;

    /** Affine matrix in Eigen. */
    MatrixXf ml(4,4);
    ml << 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;
    Affine3f t;
    Vector3f d(10,20,30);
    t.linear() = ml.transpose().block(0,0,3,3);
    t.translation() = ml.block(0,3,3,1);
    cout<<"Affine: "<<endl<<t.affine()<<endl;
    Vector3f tt(1,2,3);
    t.translation() += tt;
    cout<<"Affine after translation: "<<endl<<t.affine()<<endl;


    /** Blocks. */
    MatrixXf matr(3,2);
    matr << 1,2,3,4,5,6;
    MatrixXf combo(4,2);
    combo <<matr, MatrixXf::Ones(1,matr.cols());
    cout<<"Matr = "<<matr<<endl;
    cout<<"Comb = "<<combo<<endl;

    MatrixXf rrr = combo.block(0,0,3,2);
    cout<<"rrr = "<<rrr<<endl;


    /** Filling up a matrix*/
    std::cout<<"---------------------------------------"<<std::endl;
    MatrixXf matF(5,3);
    Vector3f vF(1,.3,3);
    Vector3f vF1(.123,.2,.3);
    Vector3f vF2(33.4,23.3,12.07);
    Vector3f vF3(0.54,8.96,14.3);
    Vector3f vF4(8.9,0.34,32.2);

    matF.row(0) = vF;
    matF.row(1) = vF1;
    matF.row(2) = vF2;
    matF.row(3) = vF3;
    matF.row(4) = vF4;

    RowVector3f means = matF.colwise().sum()/5;
    MatrixXf centered = (matF.rowwise() - means);

    cout<<"Matrix Fill = \n"<<matF<<std::endl;
    cout<<"Means = \n"<<means<<std::endl;
    cout<<"Centered = \n"<<centered<<std::endl;

    Eigen::JacobiSVD<MatrixXf> svd(centered / sqrt(5), ComputeFullV);
    cout << "Its singular values are:" << endl << svd.singularValues() << endl;
    cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
    MatrixXf V   = svd.matrixV();
    Vector3f f1  = V.col(0);
    Vector3f  f2 = V.col(1);
    Vector3f  f3 = V.col(2);
    cout << "f1(0)" << f1(0) << endl;

    VectorXf faa(V.rows());
    faa.setZero();
    //for (int i= 0; i < V.rows(); i++)
    V.col(2) = faa;
    cout << "V " << V<< endl;


    cout << "<v1,v2>" << f1.dot(f2) << endl;
    cout << "<v1,v3>" << f1.dot(f3) << endl;
    cout << "<v3,v2>" << f3.dot(f2) << endl;


    std::cout<<"---------------------------------------"<<std::endl;

    Vector3f o3(1,2,3), o2(4,5,6);
    cout << "outer? : "<< o3.cwiseProduct(o2) <<endl;

    MatrixXd mxx(3,3);
    mxx << 1,0,0,0,2,0,0,0,3;
    cout << "mxx : "<< endl<<mxx <<endl;

    vector<double> v31(3);
    VectorXd::Map(&v31[0], 3) = mxx.row(0);

    cout << "v: "<<endl;
    for(int i=0; i < 3; i++)
        cout<< v31[i]<< " " <<endl;
    cout<<endl;

    v31[1] = 100;
    cout << "v: "<<endl;
    for(int i=0; i < 3; i++)
        cout << v31[i]<< " " <<endl;
    cout<<endl;

    cout << " mxx : "<<endl <<mxx<<endl;

    Matrix3d tmat = Matrix3d::Identity();
    tmat(0,0) = 0;
    tmat(1,1) = 10;
    tmat(1,2) = 20;
    cout << "tmat: " << tmat<<endl;
    cout << "------------\n";
    for (unsigned i=0; i < tmat.rows(); i++) {
        double sum = tmat.row(i).sum() + numeric_limits<double>::min();
        tmat.row(i) /= sum;
    }

    cout << tmat << endl;

    MatrixXd src(4,3);
    src<< 0,0,0,1,1,1,2,2,2,3,3,3;
    MatrixXd target(1,3);
    target<< 1,1,1;

    cout <<"cloud err: "<<  (src.rowwise() -target.row(0)).rowwise().squaredNorm().transpose()<<endl;


    return 0;
}