void DebugOutput3DWrapper::publishTrackedFrame(Frame* kf) { KeyFrameMessage fMsg; fMsg.id = kf->id(); fMsg.time = kf->timestamp(); fMsg.isKeyframe = false; memcpy(fMsg.camToWorld.data(),kf->getScaledCamToWorld().cast<float>().data(),sizeof(float)*7); fMsg.fx = kf->fx(publishLvl); fMsg.fy = kf->fy(publishLvl); fMsg.cx = kf->cx(publishLvl); fMsg.cy = kf->cy(publishLvl); fMsg.width = kf->width(publishLvl); fMsg.height = kf->height(publishLvl); /*fMsg.pointcloud.clear(); liveframe_publisher.publish(fMsg);*/ SE3 camToWorld = se3FromSim3(kf->getScaledCamToWorld()); /*geometry_msgs::PoseStamped pMsg; pMsg.pose.position.x = camToWorld.translation()[0]; pMsg.pose.position.y = camToWorld.translation()[1]; pMsg.pose.position.z = camToWorld.translation()[2]; pMsg.pose.orientation.x = camToWorld.so3().unit_quaternion().x(); pMsg.pose.orientation.y = camToWorld.so3().unit_quaternion().y(); pMsg.pose.orientation.z = camToWorld.so3().unit_quaternion().z(); pMsg.pose.orientation.w = camToWorld.so3().unit_quaternion().w(); if (pMsg.pose.orientation.w < 0) { pMsg.pose.orientation.x *= -1; pMsg.pose.orientation.y *= -1; pMsg.pose.orientation.z *= -1; pMsg.pose.orientation.w *= -1; } pMsg.header.stamp = ros::Time(kf->timestamp()); pMsg.header.frame_id = "world"; pose_publisher.publish(pMsg);*/ cv::circle(tracker_display, cv::Point(320+camToWorld.translation()[0]*640, -240 + camToWorld.translation()[1]*480), 2, cv::Scalar(255, 0, 0),4); cv::imshow("Tracking_output", tracker_display); std::cout << "PublishTrackedKeyframe: " << camToWorld.translation()[0] << " " << camToWorld.translation()[1] << " " << camToWorld.translation()[2] << std::endl; }
void ROSOutput3DWrapper::publishTrackedFrame(Frame* kf) { lsd_slam_viewer::keyframeMsg fMsg; fMsg.id = kf->id(); fMsg.time = kf->timeStampNs()/1000000.0; fMsg.isKeyframe = false; memcpy(fMsg.camToWorld.data(),kf->getScaledCamToWorld().cast<float>().data(),sizeof(float)*7); fMsg.fx = kf->fx(publishLvl); fMsg.fy = kf->fy(publishLvl); fMsg.cx = kf->cx(publishLvl); fMsg.cy = kf->cy(publishLvl); fMsg.width = kf->width(publishLvl); fMsg.height = kf->height(publishLvl); fMsg.pointcloud.clear(); liveframe_publisher.publish(fMsg); SE3 camToWorld = se3FromSim3(kf->getScaledCamToWorld()); geometry_msgs::PoseStamped pMsg; pMsg.pose.position.x = camToWorld.translation()[0]; pMsg.pose.position.y = camToWorld.translation()[1]; pMsg.pose.position.z = camToWorld.translation()[2]; pMsg.pose.orientation.x = camToWorld.so3().unit_quaternion().x(); pMsg.pose.orientation.y = camToWorld.so3().unit_quaternion().y(); pMsg.pose.orientation.z = camToWorld.so3().unit_quaternion().z(); pMsg.pose.orientation.w = camToWorld.so3().unit_quaternion().w(); if (pMsg.pose.orientation.w < 0) { pMsg.pose.orientation.x *= -1; pMsg.pose.orientation.y *= -1; pMsg.pose.orientation.z *= -1; pMsg.pose.orientation.w *= -1; } pMsg.header.stamp = ros::Time(kf->timeStampNs()/1000000.0); pMsg.header.frame_id = "world"; pose_publisher.publish(pMsg); }
Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const { Eigen::Matrix <double,6,3> X_subspace; X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation (); X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation (); return X_subspace; }
Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const { /* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */ Eigen::Matrix<double,6,1> res; res.tail<3>() = m.rotation() * axis; res.head<3>() = m.translation().cross(res.tail<3>()); return res; }
Eigen::Matrix <Scalar_t,6,3> se3Action (const SE3 & m) const { Eigen::Matrix <double,6,3> X_subspace; X_subspace.block <3,2> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ().leftCols <2> (); X_subspace.block <3,1> (Motion::LINEAR, 2) = m.rotation ().rightCols <1> (); X_subspace.block <3,2> (Motion::ANGULAR, 0) = m.rotation ().leftCols <2> (); X_subspace.block <3,1> (Motion::ANGULAR, 2).setZero (); return X_subspace; }
void Visualizer::visualizeMarkers( const FramePtr& frame, const set<FramePtr>& core_kfs, const Map& map, bool inited, double svo_scale, double our_scale) { if((frame == NULL) || !inited) { vk::output_helper::publishTfTransform( // SE3(Matrix3d::Identity(), Vector3d::Zero()), T_world_from_vision_, ros::Time(frame->timestamp_), "odom", "cam_pos", br_); return; } SE3 temp = (frame->T_f_w_*T_world_from_vision_.inverse()).inverse(); double scale = our_scale / svo_scale; temp.translation() = temp.translation()* scale; vk::output_helper::publishTfTransform( temp, ros::Time(frame->timestamp_), "odom", "cam_pos", br_); if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0) { vk::output_helper::publishHexacopterMarker( pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_), 1, 0, 0.3, Vector3d(0.,0.,1.)); vk::output_helper::publishPointMarker( pub_points_, T_world_from_vision_*frame->pos(), "trajectory", ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5)); if(frame->isKeyframe() || publish_map_every_frame_) publishMapRegion(core_kfs); removeDeletedPts(map); } }
bool depthFromTriangulation( const SE3& T_search_ref, const Vector3d& f_ref, const Vector3d& f_cur, double& depth) { Matrix<double,3,2> A; A << T_search_ref.rotationMatrix() * f_ref, f_cur; const Matrix2d AtA = A.transpose()*A; if(AtA.determinant() < 0.000001) return false; const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation(); depth = fabs(depth2[0]); return true; }
double DepthFilter::computeTau( const SE3& T_ref_cur, const Vector3d& f, const double z, const double px_error_angle) { Vector3d t(T_ref_cur.translation()); Vector3d a = f*z-t; double t_norm = t.norm(); double a_norm = a.norm(); double alpha = acos(f.dot(t)/t_norm); // dot product double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product double beta_plus = beta + px_error_angle; double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines return (z_plus - z); // tau }
static inline void writeSE3(YAMLWriter& writer, const SE3& value) { writer.startFlowStyleListing(); const Vector3& p = value.translation(); writer.putScalar(p.x()); writer.putScalar(p.y()); writer.putScalar(p.z()); const Quat& q = value.rotation(); writer.putScalar(q.w()); writer.putScalar(q.x()); writer.putScalar(q.y()); writer.putScalar(q.z()); writer.endListing(); }
bool BodyItemImpl::store(Archive& archive) { archive.setDoubleFormat("% .6f"); archive.writeRelocatablePath("modelFile", self->filePath()); archive.write("currentBaseLink", (currentBaseLink ? currentBaseLink->name() : ""), DOUBLE_QUOTED); /// \todo Improve the following for current / initial position representations write(archive, "rootPosition", body->rootLink()->p()); write(archive, "rootAttitude", Matrix3(body->rootLink()->R())); Listing* qs = archive.createFlowStyleListing("jointPositions"); int n = body->numJoints(); for(int i=0; i < n; ++i){ qs->append(body->joint(i)->q(), 10, n); } //! \todo replace the following code with the ValueTree serialization function of BodyState SE3 initialRootPosition; if(initialState.getRootLinkPosition(initialRootPosition)){ write(archive, "initialRootPosition", initialRootPosition.translation()); write(archive, "initialRootAttitude", Matrix3(initialRootPosition.rotation())); } BodyState::Data& initialJointPositions = initialState.data(BodyState::JOINT_POSITIONS); if(!initialJointPositions.empty()){ qs = archive.createFlowStyleListing("initialJointPositions"); for(int i=0; i < initialJointPositions.size(); ++i){ qs->append(initialJointPositions[i], 10, n); } } write(archive, "zmp", zmp); if(isOriginalModelStatic != body->isStaticModel()){ archive.write("staticModel", body->isStaticModel()); } archive.write("selfCollisionDetection", isSelfCollisionDetectionEnabled); archive.write("isEditable", isEditable); return true; }
void LiveSLAMWrapper::logCameraPose(const SE3& camToWorld, double time) { Sophus::Quaternionf quat = camToWorld.unit_quaternion().cast<float>(); Eigen::Vector3f trans = camToWorld.translation().cast<float>(); char buffer[1000]; int num = snprintf(buffer, 1000, "%f %f %f %f %f %f %f %f\n", time, trans[0], trans[1], trans[2], quat.x(), quat.y(), quat.z(), quat.w()); if(outFile == 0) outFile = new std::ofstream(outFileName.c_str()); outFile->write(buffer,num); outFile->flush(); }
ForceTpl se3ActionInverse_impl(const SE3 & m) const { return ForceTpl(m.rotation().transpose()*linear_impl(), m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) ); }
/// af = aXb.act(bf) ForceTpl se3Action_impl(const SE3 & m) const { Vector3 Rf (static_cast<Vector3>( (m.rotation()) * linear_impl() ) ); return ForceTpl(Rf,m.translation().cross(Rf)+m.rotation()*angular_impl()); }
Sim3 Sim3 ::from_SE3(const SE3 & se3) { return Sim3(ScSO3(1.,se3.so3()),se3.translation()); }
void SlamSystem::setReprojectionListRelateToLastestKeyFrame(int begin, int end, Frame* current, const Eigen::Matrix3d& R_i_2_c, const Eigen::Vector3d& T_i_2_c ) { int num = end - begin; if ( num < 0 ){ num += slidingWindowSize ; } int trackFrameCnt = 0 ; for (int i = 0; i < num; i++) { int ref_id = begin + i; if (ref_id >= slidingWindowSize) { ref_id -= slidingWindowSize; } if ( slidingWindow[ref_id]->keyFrameFlag == false || trackFrameCnt > 10 ){ continue; } double closenessTH = 1.0 ; double distanceTH = closenessTH * 15 / (KFDistWeight*KFDistWeight); //double cosAngleTH = 1.0 - 0.25 * closenessTH ; //euclideanOverlapCheck double distFac = slidingWindow[ref_id]->meanIdepth ; Eigen::Vector3d dd = ( slidingWindow[ref_id]->T_bk_2_b0 - current->T_bk_2_b0) * distFac; if( dd.dot(dd) > distanceTH) continue; // Eigen::Quaterniond qq( slidingWindow[ref_id]->R_bk_2_b0.transpose() * current->R_bk_2_b0) ; // Eigen::Vector3d aa = qq.vec()*2.0 ; // double dirDotProd = aa.dot( aa ) ; // if(dirDotProd < cosAngleTH) continue; Matrix3d R_i_2_j ; Vector3d T_i_2_j ; SE3 c2f_init ; //check from current to ref R_i_2_j = slidingWindow[ref_id]->R_bk_2_b0.transpose() * current->R_bk_2_b0 ; T_i_2_j = -slidingWindow[ref_id]->R_bk_2_b0.transpose() * ( slidingWindow[ref_id]->T_bk_2_b0 - current->T_bk_2_b0 ) ; c2f_init.setRotationMatrix(R_i_2_j); c2f_init.translation() = T_i_2_j ; trackerConstraint->trackFrameOnPermaref(current, slidingWindow[ref_id].get(), c2f_init ) ; if ( trackerConstraint->trackingWasGood == false ){ //ROS_WARN("first check fail") ; continue ; } //ROS_WARN("pass first check") ; //check from ref to current R_i_2_j = current->R_bk_2_b0.transpose() * slidingWindow[ref_id]->R_bk_2_b0 ; T_i_2_j = -current->R_bk_2_b0.transpose() * ( current->T_bk_2_b0 - slidingWindow[ref_id]->T_bk_2_b0 ) ; c2f_init.setRotationMatrix(R_i_2_j); c2f_init.translation() = T_i_2_j ; trackerConstraint->trackFrameOnPermaref(slidingWindow[ref_id].get(), current, c2f_init ) ; if ( trackerConstraint->trackingWasGood == false ){ //ROS_WARN("second check fail") ; continue ; } //ROS_WARN("pass second check") ; //Pass the cross check if ( trackingReferenceConstraint->keyframe != slidingWindow[ref_id].get() ){ trackingReferenceConstraint->importFrame( slidingWindow[ref_id].get() ); } SE3 RefToFrame = trackerConstraint->trackFrame( trackingReferenceConstraint, current, c2f_init ); trackFrameCnt++ ; //float tracking_lastResidual = trackerConstraint->lastResidual; //float tracking_lastUsage = trackerConstraint->pointUsage; //float tracking_lastGoodPerBad = trackerConstraint->lastGoodCount / (trackerConstraint->lastGoodCount + trackerConstraint->lastBadCount); float tracking_lastGoodPerTotal = trackerConstraint->lastGoodCount / (current->width(SE3TRACKING_MIN_LEVEL)*current->height(SE3TRACKING_MIN_LEVEL)); Sophus::Vector3d dist = RefToFrame.translation() * slidingWindow[ref_id]->meanIdepth; float minVal = 1.0f; float lastTrackingClosenessScore = getRefFrameScore(dist.dot(dist), trackerConstraint->pointUsage, KFDistWeight, KFUsageWeight); if ( trackerConstraint->trackingWasGood == false || tracking_lastGoodPerTotal < MIN_GOODPERALL_PIXEL || lastTrackingClosenessScore > minVal ) { continue ; } #ifdef PROJECT_TO_IMU_CENTER Eigen::Matrix3d r_i_2_j = RefToFrame.rotationMatrix().cast<double>(); Eigen::Vector3d t_i_2_j = RefToFrame.translation().cast<double>(); Eigen::Matrix3d final_R = R_i_2_c.transpose()*r_i_2_j*R_i_2_c; Eigen::Vector3d final_T = R_i_2_c.transpose()*(r_i_2_j*T_i_2_c + t_i_2_j ) - R_i_2_c.transpose()*T_i_2_c ; #else Eigen::Matrix3d final_R = RefToFrame.rotationMatrix().cast<double>(); Eigen::Vector3d final_T = RefToFrame.translation().cast<double>(); #endif //ROS_WARN("[add link, from %d to %d]", slidingWindow[ref_id]->id(), current->id() ) ; insertCameraLink( slidingWindow[ref_id].get(), current, final_R, final_T, MatrixXd::Identity(6, 6)*DENSE_TRACKING_WEIGHT ) ; break ; } }