C++ Vector3f::data方法代码示例

	bool update_map(rm_localization::UpdateMap::Request &req,
			rm_localization::UpdateMap::Response &res) {

		boost::mutex::scoped_lock lock(closest_keyframe_update_mutex);

		Eigen::Vector3f intrinsics;
		memcpy(intrinsics.data(), req.intrinsics.data(), 3 * sizeof(float));

		bool update_intrinsics = intrinsics[0] != 0.0f;

		if (update_intrinsics) {
			ROS_INFO("Updated camera intrinsics");
			this->intrinsics = intrinsics;
			ROS_INFO_STREAM("New intrinsics" << std::endl << this->intrinsics);
		}

		for (size_t i = 0; i < req.idx.size(); i++) {

			Eigen::Quaternionf orientation;
			Eigen::Vector3f position, intrinsics;

			memcpy(orientation.coeffs().data(),
					req.transform[i].unit_quaternion.data(), 4 * sizeof(float));
			memcpy(position.data(), req.transform[i].position.data(),
					3 * sizeof(float));

			Sophus::SE3f new_pos(orientation, position);

			if (req.idx[i] == closest_keyframe_idx) {
				//closest_keyframe_update_mutex.lock();

				camera_position = new_pos
						* keyframes[req.idx[i]]->get_pos().inverse()
						* camera_position;

				keyframes[req.idx[i]]->get_pos() = new_pos;

				//if (update_intrinsics) {
				//	keyframes[req.idx[i]]->get_intrinsics() = intrinsics;
				//}

				//closest_keyframe_update_mutex.unlock();

			} else {
				keyframes[req.idx[i]]->get_pos() = new_pos;

				//if (update_intrinsics) {
				//	keyframes[req.idx[i]]->get_intrinsics() = intrinsics;
				//}

			}
		}

		return true;
	}

void RGBDOdometry::initICPModel(GPUTexture * predictedVertices,
                                GPUTexture * predictedNormals,
                                const float depthCutoff,
                                const Eigen::Matrix4f & modelPose)
{
    cudaArray * textPtr;

    cudaGraphicsMapResources(1, &predictedVertices->cudaRes);
    cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedVertices->cudaRes, 0, 0);
    cudaMemcpyFromArray(vmaps_tmp.ptr(), textPtr, 0, 0, vmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice);
    cudaGraphicsUnmapResources(1, &predictedVertices->cudaRes);

    cudaGraphicsMapResources(1, &predictedNormals->cudaRes);
    cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedNormals->cudaRes, 0, 0);
    cudaMemcpyFromArray(nmaps_tmp.ptr(), textPtr, 0, 0, nmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice);
    cudaGraphicsUnmapResources(1, &predictedNormals->cudaRes);

    copyMaps(vmaps_tmp, nmaps_tmp, vmaps_g_prev_[0], nmaps_g_prev_[0]);

    for(int i = 1; i < NUM_PYRS; ++i)
    {
        resizeVMap(vmaps_g_prev_[i - 1], vmaps_g_prev_[i]);
        resizeNMap(nmaps_g_prev_[i - 1], nmaps_g_prev_[i]);
    }

    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3);
    Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1);

    mat33 device_Rcam = Rcam;
    float3 device_tcam = *reinterpret_cast<float3*>(tcam.data());

    for(int i = 0; i < NUM_PYRS; ++i)
    {
        tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
    }

    cudaDeviceSynchronize();
}

void display()
{
  using namespace Eigen;
  using namespace igl;
  using namespace std;
  glClearColor(back[0],back[1],back[2],0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  // All smooth points
  glEnable( GL_POINT_SMOOTH );

  lights();
  push_scene();
  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_NORMALIZE);
  glEnable(GL_COLOR_MATERIAL);
  glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
  push_object();

  if(trackball_on)
  {
    // Draw a "laser" line
    glLineWidth(3.0);
    glDisable(GL_LIGHTING);
    glEnable(GL_DEPTH_TEST);
    glBegin(GL_LINES);
    glColor3f(1,0,0);
    glVertex3fv(s.data());
    glColor3f(1,0,0);
    glVertex3fv(d.data());
    glEnd();

    // Draw the start and end points used for ray
    glPointSize(10.0);
    glBegin(GL_POINTS);
    glColor3f(1,0,0);
    glVertex3fv(s.data());
    glColor3f(0,0,1);
    glVertex3fv(d.data());
    glEnd();
  }

  // Draw the model
  glEnable(GL_LIGHTING);
  draw_mesh(V,F,N,C);

  // Draw all hits
  glBegin(GL_POINTS);
  glColor3f(0,0.2,0.2);
  for(vector<igl::embree::Hit>::iterator hit = hits.begin();
      hit != hits.end();
      hit++)
  {
    const double w0 = (1.0-hit->u-hit->v);
    const double w1 = hit->u;
    const double w2 = hit->v;
    VectorXd hitP =
      w0 * V.row(F(hit->id,0)) +
      w1 * V.row(F(hit->id,1)) +
      w2 * V.row(F(hit->id,2));
    glVertex3dv(hitP.data());
  }
  glEnd();

  pop_object();

  // Draw a nice floor
  glPushMatrix();
  glEnable(GL_LIGHTING);
  glTranslated(0,-1,0);
  draw_floor();
  glPopMatrix();

  // draw a transparent "projection screen" show model at time of hit (aka
  // mouse down)
  push_object();
  if(trackball_on)
  {
    glColor4f(0,0,0,1.0);
    glPointSize(10.0);
    glBegin(GL_POINTS);
    glVertex3fv(SW.data());
    glVertex3fv(SE.data());
    glVertex3fv(NE.data());
    glVertex3fv(NW.data());
    glEnd();

    glDisable(GL_LIGHTING);
    glEnable(GL_TEXTURE_2D);
    glBindTexture(GL_TEXTURE_2D, tex_id);
    glColor4f(1,1,1,0.7);
    glBegin(GL_QUADS);
    glTexCoord2d(0,0);
    glVertex3fv(SW.data());
    glTexCoord2d(1,0);
    glVertex3fv(SE.data());
    glTexCoord2d(1,1);
    glVertex3fv(NE.data());
    glTexCoord2d(0,1);
    glVertex3fv(NW.data());
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

void set_uniform_vector(GLuint programID, const char* NAME, const Eigen::Vector3f& vector) {
    GLuint matrix_id = glGetUniformLocation(programID, NAME);
    glUniform3fv(matrix_id, 1, vector.data());
}

	void update_map(bool with_intrinsics = false) {

		rm_localization::UpdateMap update_map_msg;
		update_map_msg.request.idx.resize(map->frames.size());
		update_map_msg.request.transform.resize(map->frames.size());

		if (with_intrinsics) {

			Eigen::Vector3f intrinsics = map->frames[0]->get_intrinsics();

			sensor_msgs::SetCameraInfo s;
			s.request.camera_info.width = map->frames[0]->get_i(0).cols;
			s.request.camera_info.height = map->frames[0]->get_i(0).rows;

			// No distortion
			s.request.camera_info.D.resize(5, 0.0);
			s.request.camera_info.distortion_model =
					sensor_msgs::distortion_models::PLUMB_BOB;

			// Simple camera matrix: square pixels (fx = fy), principal point at center
			s.request.camera_info.K.assign(0.0);
			s.request.camera_info.K[0] = s.request.camera_info.K[4] =
					intrinsics[0];
			s.request.camera_info.K[2] = intrinsics[1];
			s.request.camera_info.K[5] = intrinsics[2];
			s.request.camera_info.K[8] = 1.0;

			// No separate rectified image plane, so R = I
			s.request.camera_info.R.assign(0.0);
			s.request.camera_info.R[0] = s.request.camera_info.R[4] =
					s.request.camera_info.R[8] = 1.0;

			// Then P=K(I|0) = (K|0)
			s.request.camera_info.P.assign(0.0);
			s.request.camera_info.P[0] = s.request.camera_info.P[5] =
					s.request.camera_info.K[0]; // fx, fy
			s.request.camera_info.P[2] = s.request.camera_info.K[2]; // cx
			s.request.camera_info.P[6] = s.request.camera_info.K[5]; // cy
			s.request.camera_info.P[10] = 1.0;

			set_camera_info_service.call(s);

			memcpy(update_map_msg.request.intrinsics.data(), intrinsics.data(),
					3 * sizeof(float));
		} else {
			update_map_msg.request.intrinsics = { {0,0,0}};
		}

		for (size_t i = 0; i < map->frames.size(); i++) {
			update_map_msg.request.idx[i] = map->idx[i];

			Sophus::SE3f position = map->frames[i]->get_pos();

			memcpy(update_map_msg.request.transform[i].unit_quaternion.data(),
					position.unit_quaternion().coeffs().data(),
					4 * sizeof(float));

			memcpy(update_map_msg.request.transform[i].position.data(),
					position.translation().data(), 3 * sizeof(float));

		}

		update_map_service.call(update_map_msg);

	}