SE3<> IMU2camWorldfromQuat(Eigen::Quaternion<double> atti){// use ini attitude info from EKF node to ini ptam pose Vector<4> attivec = makeVector(atti.w(), atti.x(), atti.y(), atti.z());//Rw1i Matrix<3> iniOrientationEKF; iniOrientationEKF = tag::quaternionToMatrix(attivec); Matrix<3> roll = TooN::Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in 0, -1, 0, // a world frame which pointing downward. 0, 0, -1); SE3<> camWorld = SE3<>(); Matrix<3> rotation; if (tracker_->attitude_get) rotation = iniOrientationEKF; // else rotation = roll * iniOrientationEKF;//Rwi = Rww1*Rw1i camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();//Rwc = (Rwi * Ric) Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation(); camWorld.get_translation()[0] = 0.0;//twc[0]; //twc camWorld.get_translation()[1] = 0.0;//twc[1]; camWorld.get_translation()[2] = twc[2]; camWorld = camWorld.inverse();//Tcw cout<< "TCW INITIALIZED. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl; // cout<< camWorld.get_rotation().get_matrix()(2,2)<<endl; return camWorld; }
void SyntheticDataGenerator::GenerateTipTrajectory() { this->tipTrajectory.resize(this->jointTrajectory.size()); SE3 tipFrame; for(int i = 0 ; i < this->jointTrajectory.size(); ++i) { double* rotation = this->jointTrajectory[i].data() + 1; double* translation = this->jointTrajectory[i].data() + 1 + this->robot->GetNumOfTubes(); //if(kinematics->ComputeKinematics(rotation, translation)) //std::cout << "Solved!" << std::endl; if(!kinematics->ComputeKinematics(rotation, translation)) std::cout << rotation[0] << "\t" << rotation[1] << "\t" << rotation[2] << "\t" << translation[0] << "\t" << translation[1] << "\t" << translation[2] << std::endl; kinematics->GetBishopFrame(tipFrame); this->tipTrajectory[i].resize(12); for(int j = 0; j < 3; ++j) { this->tipTrajectory[i][j] = tipFrame.GetPosition()[j]; this->tipTrajectory[i][j+3] = tipFrame.GetOrientation().GetX()[j]; this->tipTrajectory[i][j+6] = tipFrame.GetOrientation().GetY()[j]; this->tipTrajectory[i][j+9] = tipFrame.GetOrientation().GetZ()[j]; } } }
// get Tcw SE3<> imu2camWorld(pximu::AttitudeData attitude_data){ Matrix<3> Rroll = Data(1.0, 0, 0,//roll Rot(-roll)T=Rot(roll) 0, cos(attitude_data.roll), -sin(attitude_data.roll), 0, sin(attitude_data.roll), cos(attitude_data.roll)); Matrix<3> Rpitch = Data(cos(attitude_data.pitch), 0, -sin(attitude_data.pitch),//pitch Rot(-pitch)T=Rot(pitch) 0, 1.0, 0, sin(attitude_data.pitch), 0, cos(attitude_data.pitch)); Matrix<3> roll = Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in 0, -1, 0, // a world frame which pointing downward. 0, 0, -1); SE3<> camWorld = SE3<>(); Matrix<3> rotation = roll * Rpitch * Rroll; //Rwr*Rrc-->Rwc camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix(); Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation(); camWorld.get_translation()[0] = 0.0;//twc[0]; //twc camWorld.get_translation()[1] = 0.0;//twc[1]; // TODO: find out the bug why must use 0 camWorld.get_translation()[2] = twc[2]; camWorld = camWorld.inverse();//Tcw cout<< "TCW INITIALIZED FROM IMU ATT. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl; return camWorld; }
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 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; }
string serializeForOutput(SE3<double> pose) { stringstream so; Vector<3>& trans=pose.get_translation(); Matrix<3, 3> rot=pose.get_rotation().get_matrix(); // row major // output in column-major - for matlab! just use reshape(x,3,4) for (uint32_t j = 0; j < 3; ++j) for (uint32_t i = 0; i < 3; ++i) so << rot(i, j) << "\t"; for (uint32_t i = 0; i < 3; ++i) so << trans[i] << "\t"; return so.str(); }
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; }
int main(int argc, char ** argv){ test(); #if 0 SE3<> id(makeVector(1,0,0,0,0,0)); const SE3<F<double> > g = make_left_fad_se3(id); for(int i = 0; i < 6; ++i) cout << get_derivative(g.get_rotation().get_matrix(), i) << get_derivative(g.get_translation(), i) << "\n\n"; Vector<3> in = makeVector(1,2,3); const Vector<3, F<double> > p = g * in; cout << p << "\n" << get_jacobian<3,6>(p) << endl; #endif }
void SlamGraph<SE3,StereoCamera, SE3XYZ_STEREO, 3> ::addConstraintToG2o(const SE3 & T_2_from_1, const Matrix6d & Lambda_2_from_1, int pose_id_1, int pose_id_2, g2o::SparseOptimizer * optimizer) { G2oEdgeSE3 * e = new G2oEdgeSE3(); g2o::HyperGraph::Vertex * pose1_vertex = GET_MAP_ELEM(pose_id_1, optimizer->vertices()); e->vertices()[0] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose1_vertex); g2o::HyperGraph::Vertex * pose2_vertex = GET_MAP_ELEM(pose_id_2, optimizer->vertices()); e->vertices()[1] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose2_vertex); e->measurement() = T_2_from_1; e->inverseMeasurement() = T_2_from_1.inverse(); e->information() = Lambda_2_from_1; optimizer->addEdge(e); }
Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const { Eigen::Matrix<double,6,1> res; res.head<3>() = m.rotation().col(axis); res.tail<3>() = Motion::Vector3::Zero(); return res; }
bool updateDepthFilter( const Vector2d& pt_ref, const Vector2d& pt_curr, const SE3& T_C_R, Mat& depth, Mat& depth_cov ) { // 我是一只喵 // 不知道这段还有没有人看 // 用三角化计算深度 SE3 T_R_C = T_C_R.inverse(); Vector3d f_ref = px2cam( pt_ref ); f_ref.normalize(); Vector3d f_curr = px2cam( pt_curr ); f_curr.normalize(); // 方程参照本书第 7 讲三角化一节 Vector3d t = T_R_C.translation(); Vector3d f2 = T_R_C.rotation_matrix() * f_curr; Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) ); double A[4]; A[0] = f_ref.dot ( f_ref ); A[2] = f_ref.dot ( f2 ); A[1] = -A[2]; A[3] = - f2.dot ( f2 ); double d = A[0]*A[3]-A[1]*A[2]; Vector2d lambdavec = Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ), -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d; Vector3d xm = lambdavec ( 0,0 ) * f_ref; Vector3d xn = t + lambdavec ( 1,0 ) * f2; Vector3d d_esti = ( xm+xn ) / 2.0; // 三角化算得的深度向量 double depth_estimation = d_esti.norm(); // 深度值 // 计算不确定性(以一个像素为误差) Vector3d p = f_ref*depth_estimation; Vector3d a = p - t; double t_norm = t.norm(); double a_norm = a.norm(); double alpha = acos( f_ref.dot(t)/t_norm ); double beta = acos( -a.dot(t)/(a_norm*t_norm)); double beta_prime = beta + atan(1/fx); double gamma = M_PI - alpha - beta_prime; double p_prime = t_norm * sin(beta_prime) / sin(gamma); double d_cov = p_prime - depth_estimation; double d_cov2 = d_cov*d_cov; // 高斯融合 double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ]; double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ]; double mu_fuse = (d_cov2*mu+sigma2*depth_estimation) / ( sigma2+d_cov2); double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 ); depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse; depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2; return true; }
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); }
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(); }
Matrix6d third(const SE3 & A, const Vector6d & d) { const Matrix6d & AdjA = A.Adj(); Matrix6d d_lie = SE3::d_lieBracketab_by_d_a(d); //cerr << d_lie << endl; return AdjA + 0.5*d_lie*AdjA + (1./12.)*d_lie*d_lie*AdjA; }
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(); }
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); } }
// the second camera pose in the master camera frames SE3<> Load_camsec_para() { SE3<> campara; ifstream cam_para_table; string table_path = cam_para_path_second; double temp1, temp2, temp3, temp4, temp5; cam_para_table.open(table_path.c_str()); assert(cam_para_table.is_open()); // cam_para_table>>temp1>>temp2>>temp3>>temp4>>temp5; Matrix<3> cam; for (int i = 0; i < 3; i ++){ for (int j = 0; j < 3; j ++){ cam_para_table>>temp1; cam(i, j) = temp1; } cam_para_table>>temp1; campara.get_translation()[i] = temp1/1000.0; } campara.get_rotation() = cam; return campara;//Tic }
void ARDriver::Render(Image<Rgb<byte> >& imFrame, SE3<> se3CfromW) { if (!mbInitialised) { Init(); Reset(); }; mnCounter++; // Upload the image to our frame texture glBindTexture(GL_TEXTURE_RECTANGLE_ARB, mnFrameTex); glTexSubImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, 0, 0, mirFrameSize.x, mirFrameSize.y, GL_RGB, GL_UNSIGNED_BYTE, imFrame.data()); // Set up rendering to go the FBO, draw undistorted video frame into BG glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, mnFrameBuffer); CheckFramebufferStatus(); glViewport(0, 0, mirFBSize.x, mirFBSize.y); DrawFBBackGround(); glClearDepth(1); glClear(GL_DEPTH_BUFFER_BIT); // Set up 3D projection glMatrixMode(GL_PROJECTION); glLoadIdentity(); glMultMatrix(mCamera.MakeUFBLinearFrustumMatrix(0.005, 100)); glMultMatrix(se3CfromW); DrawFadingGrid(); mGame.DrawStuff(se3CfromW.inverse().get_translation()); glDisable(GL_DEPTH_TEST); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glDisable(GL_BLEND); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); // Set up for drawing 2D stuff: glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0); DrawDistortedFB(); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); mGLWindow.SetupViewport(); mGLWindow.SetupVideoOrtho(); mGLWindow.SetupVideoRasterPosAndZoom(); }
void DisplayCallback(void) { glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(_fovy, _aspect, _zNear, _zFar); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glMultMatrixd(_mvmatrix.GetArray()); DrawModel(); DrawTextInfo(); glutSwapBuffers(); glutPostRedisplay(); glClearColor(_backgroundcolor[0],_backgroundcolor[1],_backgroundcolor[2],_backgroundcolor[3]); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); }
static bool testExponential() { SE3<double> pose; Eigen::Matrix4d S, expectedExp, closedForm; S.setZero(); expectedExp.setZero(), closedForm.setZero(); SE3<double>::ParameterVectorType delta; size_t n = 5000; bool b = true; while( n-- ){ delta[ 0 ] = Math::rand( -2.6, 2.6 ); delta[ 1 ] = Math::rand( -2.6, 2.6 ); delta[ 2 ] = Math::rand( -2.6, 2.6 ); delta[ 3 ] = Math::rand( -10.0, 10.0 ); delta[ 4 ] = Math::rand( -10.0, 10.0 ); delta[ 5 ] = Math::rand( -10.0, 10.0 ); fillExpMatrix( S, delta ); cvt::Math::exponential( S, expectedExp, 10 ); pose.evalExp( closedForm, delta ); bool res = ( ( expectedExp - closedForm ).array().abs().sum() / 12 < 0.00001 ); if( !res ){ std::cout << "Expected: \n" << expectedExp << std::endl; std::cout << "Closed Form: \n" << closedForm << std::endl; } b &= res; } CVTTEST_PRINT( "exp()", b ); return b; }
void MotionCallback(int x, int y) { bool bctrlpressed = false; if (glutGetModifiers() == GLUT_ACTIVE_CTRL) { bctrlpressed = true; } if ( _bRotateView ) { Vec3 spoint = get_pos_sp(x, y); Vec3 n = Cross(_spoint_prev, spoint); if ( Norm(n) > 1e-6 ) { if ( !bctrlpressed ) { n *= 3.0; } Vec3 w = ~(_mvmatrix_prev.GetRotation()) * n; _mvmatrix.SetRotation(_mvmatrix_prev.GetRotation() * Exp(w)); } else { return; } } if (_bTranslateView) { float den = 30; if ( bctrlpressed ) { den = 300; } _xcam += (float)(x-_downX)/den*_mscale; ycam += (float)(_downY-y)/den*_mscale; } // translate if (_bZoomInOut) { float den = 30; if ( bctrlpressed ) { den = 300; } _sdepth -= (float)(_downY-y)/den*_mscale; } // zoom in/out _downX = x; _downY = y; glutPostRedisplay(); }
bool testForceSet() { using namespace se3; typedef Eigen::Matrix<double,4,4> Matrix4; typedef SE3::Matrix6 Matrix6; typedef SE3::Vector3 Vector3; typedef Force::Vector6 Vector6; SE3 amb = SE3::Random(); SE3 bmc = SE3::Random(); SE3 amc = amb*bmc; ForceSet F(12); ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero()); F.block(10,2) = F2; assert( F.matrix().col(10).norm() == 0.0 ); assert( isnan(F.matrix()(0,9)) ); std::cout << "F10 = " << F2.matrix() << std::endl; std::cout << "F = " << F.matrix() << std::endl; ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random()); ForceSet F4 = amb.act(F3); SE3::Matrix6 aXb= amb; assert( (aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix()) ); ForceSet bF = bmc.act(F3); ForceSet aF = amb.act(bF); ForceSet aF2 = amc.act(F3); assert( aF.matrix().isApprox( aF2.matrix() ) ); ForceSet F36 = amb.act(F3.block(3,6)); assert( (aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix()) ); ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6)); assert( (aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)) .isApprox(F36full.matrix().block(0,3,6,6)) ); 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 bool testApply() { bool b, ret; // apply delta: Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero(); Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity(); Eigen::Matrix<double, 4, 4> diff; SE3<double> pose; pose.set( delta ); delta[ 0 ] = Math::deg2Rad( 1.5 ); delta[ 1 ] = Math::deg2Rad( 1.1 ); delta[ 2 ] = Math::deg2Rad( 1.6 ); delta[ 3 ] = 1; delta[ 4 ] = 1; delta[ 5 ] = 1; pose.apply( delta ); expectedT( 0, 3 ) = delta[ 3 ]; expectedT( 1, 3 ) = delta[ 4 ]; expectedT( 2, 3 ) = delta[ 5 ]; Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 ); double angle = axis.norm(); axis /= angle; expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix(); diff = expectedT - pose.transformation(); ret = b = ( diff.array().abs().sum() / 12 < 0.001 ); if( !b ){ std::cout << expectedT << std::endl; std::cout << pose.transformation() << std::endl; std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl; } pose.apply( -delta ); expectedT.setIdentity(); b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 ); CVTTEST_PRINT( "apply", b ); ret &= b; return ret; }
int main(int argc, char **argv) { InitVars(argc, argv); if (argc != 1) { DLOG << "Usage: "<<argv[0]; return 0; } // Load the map and rotate to canonical image Map map; map.load_images = false; map.Load(); Vector<3> lnR = makeVector(1.1531, 1.26237, -1.24435); // for lab_kitchen1 SE3<> scene_to_slam; scene_to_slam.get_rotation() = SO3<>::exp(lnR); map.scene_to_slam = SO3<>::exp(lnR); // TODO: map.Load() should do this automatically map.undistorter.Compute(ImageRef(640,480)); ptam::ATANCamera& cam = map.undistorter.cam; vector<int> frame_ids; ParseMultiRange(GV3::get<string>("CanonicalImages.InputFrames"), frame_ids); COUNTED_FOREACH(int i, int id, frame_ids) { if (id < 0 || id >= map.frame_specs.size()) { DLOG << "Frame index " << id << " out of range" << " (there are " << map.frame_specs.size() << "frames available)"; } else { const Frame& fs = map.frame_specs[frame_ids[i]]; if (!fs.lost) { // Read the image and detect lines ImageBundle image(fs.filename); PeakLines lines; lines.Compute(image, fs.pose, map); SE3<> pose = fs.pose * scene_to_slam; // Project vanishing points Vector<3> vpts[3]; int vert_axis = 0; for (int i = 0; i < 3; i++) { vpts[i] = pose.get_rotation().get_matrix().T()[i]; if (abs(vpts[i][1]) > abs(vpts[vert_axis][1])) { vert_axis = i; } } int l_axis = (vert_axis+1)%3; int r_axis = (vert_axis+2)%3; // Compute the rotations Matrix<3> R_l_inv, R_r_inv; R_l_inv.T()[0] = positive(vpts[l_axis], 0); R_l_inv.T()[1] = positive(vpts[vert_axis], 1); R_l_inv.T()[2] = vpts[r_axis]; R_r_inv.T()[0] = positive(vpts[r_axis], 0); R_r_inv.T()[1] = positive(vpts[vert_axis], 1); R_r_inv.T()[2] = vpts[l_axis]; Matrix<3> R_l = LU<3>(R_l_inv).get_inverse(); Matrix<3> R_r = LU<3>(R_r_inv).get_inverse(); // Warp the images ImageRGB<byte> canvas_l(image.sz()), canvas_r(image.sz()); canvas_l.Clear(PixelRGB<byte>(255,255,255)); canvas_r.Clear(PixelRGB<byte>(255,255,255)); for (int y = 0; y < image.ny(); y++) { for (int x = 0; x < image.nx(); x++) { Vector<3> dest = unproject(cam.UnProject(makeVector(x,y))); Vector<3> l_src = R_l_inv * dest; Vector<3> r_src = R_r_inv * dest; Vector<2> l_im = cam.Project(project(l_src)); Vector<2> r_im = cam.Project(project(r_src)); ImageRef l_ir = round_pos(l_im); ImageRef r_ir = round_pos(r_im); if (image.contains(l_ir)) { canvas_l[y][x] = image.rgb[l_ir]; } if (image.contains(r_ir)) { canvas_r[y][x] = image.rgb[r_ir]; } } } ImageRGB<byte> canvas; ImageCopy(image.rgb, canvas); // Read ratios double c_over_f = GV3::get<double>("CanonicalImage.LeftCOverF"); double f_over_c = 1.0 / c_over_f; // Transfer some points int horizon_y = image.ny()/2; for (int j = 0; j < 50; j++) { Vector<2> im_p1 = makeVector(rand()%image.nx(), rand()%image.ny()); Vector<2> ret_p1 = cam.UnProject(im_p1); double y2; if (ret_p1[1] > 0) { y2 = -c_over_f * ret_p1[1]; } else { y2 = -f_over_c * ret_p1[1]; } Vector<2> ret_p2 = makeVector(ret_p1[0], y2); Vector<2> im_p2 = cam.Project(ret_p2); Vector<3> orig_ret_p1 = R_l * unproject(ret_p1); Vector<3> orig_ret_p2 = R_l * unproject(ret_p2); Vector<2> orig_im_p1 = cam.Project(project(orig_ret_p1)); Vector<2> orig_im_p2 = cam.Project(project(orig_ret_p2)); DrawLineClipped(canvas, orig_im_p1, orig_im_p2, BrightColors::Get(j)); DrawLineClipped(canvas_l, im_p1, im_p2, BrightColors::Get(j)); } string basename = "out/canon"+PaddedInt(id, 4); WriteImage(basename+"_left.png", canvas_l); WriteImage(basename+"_right.png", canvas_r); WriteImage(basename+"_orig.png", canvas); } } } }
/// 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()); }
ForceTpl se3ActionInverse_impl(const SE3 & m) const { return ForceTpl(m.rotation().transpose()*linear_impl(), m.rotation().transpose()*(angular_impl() - m.translation().cross(linear_impl())) ); }