slam高博随书代码slambook-master.rar

slam高博随书代码slambook-master.rar

// This file is part of Sophus.
//
// Copyright 2012 Hauke Strasdat （Imperial College London）
//
// Permission is hereby granted free of charge to any person obtaining a copy
// of this software and associated documentation files （the “Software“） to
// deal in the Software without restriction including without limitation the
// rights  to use copy modify merge publish distribute sublicense and/or
// sell copies of the Software and to permit persons to whom the Software is
// furnished to do so subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED “AS IS“ WITHOUT WARRANTY OF ANY KIND EXPRESS OR
// IMPLIED INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM DAMAGES OR OTHER
// LIABILITY WHETHER IN AN ACTION OF CONTRACT TORT OR OTHERWISE ARISING
// FROM OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#include 
#include “scso3.h“

namespace Sophus
{

ScSO3::ScSO3（）
{
  quaternion_.setIdentity（）;
}

ScSO3
::ScSO3（const ScSO3 & other） : quaternion_（other.quaternion_） {}

ScSO3
::ScSO3（const Matrix3d & scale_times_R）
{
  Matrix3d squaredScaleMat = scale_times_R*scale_times_R.transpose（）;
  double squaredScale
      =（ 1./3.）*（squaredScaleMat（00）
                 +squaredScaleMat（11）
                 +squaredScaleMat（22））;
  assert（squaredScale>0）;
  double scale = sqrt（squaredScale）;

  quaternion_ = scale_times_R/scale;
  quaternion_.coeffs（） *= scale;
}


ScSO3
::ScSO3（double scale
        const Matrix3d & R）
{
  assert（scale>0）;
  quaternion_ = R;
  quaternion_.normalize（）;
  quaternion_.coeffs（） *= scale;
}

ScSO3
::ScSO3（double scale const SO3 & so3）
{
  assert（scale>0）;
  quaternion_ = so3.unit_quaternion（）;
  quaternion_.coeffs（） *= scale;
}

ScSO3
::ScSO3（const Quaterniond & quat） : quaternion_（quat） {}

void ScSO3
::operator=（const ScSO3 & other）
{
  this->quaternion_ = other.quaternion_;
}

ScSO3 ScSO3
::operator*（const ScSO3& other） const
{
  ScSO3 result（*this）;
  result.quaternion_ *= other.quaternion_;
  return result;
}

void ScSO3
::operator*=（const ScSO3& other）
{
  quaternion_ *= other.quaternion_;
}

Vector3d ScSO3
::operator*（const Vector3d & xyz） const
{
  //ToDO: implement this directly!
  double scale = quaternion（）.norm（）;
  Quaterniond norm_quad = quaternion_;
  norm_quad.coeffs（） /= scale;
  return scale*norm_quad._transformVector（xyz）;
}

ScSO3 ScSO3
::inverse（） const
{
  assert（quaternion_.squaredNorm（）>0.）;
  return ScSO3（quaternion（）.inverse（））;
}

Matrix3d ScSO3
::matrix（） const
{
  //ToDO: implement this directly!
  double scale = quaternion（）.norm（）;
  Quaterniond norm_quad = quaternion_;
  norm_quad.coeffs（） /= scale;
  return scale*norm_quad.toRotat

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件         66  2017-03-05 22:32  slambook-master\.gitignore

     文件   14605482  2017-03-05 22:32  slambook-master\3rdparty\ceres-solver.tar.gz

     文件    1243868  2017-03-05 22:32  slambook-master\3rdparty\DBow3.tar.gz

     文件    8342066  2017-03-05 22:32  slambook-master\3rdparty\g2o.tar.gz

     文件    3527776  2017-03-05 22:32  slambook-master\3rdparty\Pangolin.tar.gz

     文件        259  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\config

     文件         73  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\description

     文件         41  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\HEAD

     文件        452  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\applypatch-msg.sample

     文件        896  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\commit-msg.sample

     文件        189  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\post-update.sample

     文件        398  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\pre-applypatch.sample

     文件       1642  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\pre-commit.sample

     文件       1352  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\pre-push.sample

     文件       4898  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\pre-rebase.sample

     文件       1239  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\prepare-commit-msg.sample

     文件       3611  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\hooks\update.sample

     文件       1856  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\index

     文件        240  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\info\exclude

     文件        374  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\logs\HEAD

     文件        191  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\logs\refs\heads\master

     文件        191  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\logs\refs\remotes\origin\HEAD

     文件      20840  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\objects\pack\pack-8af7d0c082c2aba1075d410cd3cd9c8b987ecf83.idx

     文件     242441  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\objects\pack\pack-8af7d0c082c2aba1075d410cd3cd9c8b987ecf83.pack

     文件        242  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\packed-refs

     文件         41  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\refs\heads\master

     文件         32  2016-07-04 10:13  slambook-master\3rdparty\Sophus\.git\refs\remotes\origin\HEAD

     文件       2881  2016-07-04 10:13  slambook-master\3rdparty\Sophus\CMakeLists.txt

     文件        844  2016-07-04 10:13  slambook-master\3rdparty\Sophus\cmake_modules\FindEigen3.cmake

     文件        143  2016-07-04 10:13  slambook-master\3rdparty\Sophus\README

............此处省略377个文件信息