pose_3d.cpp 6.92 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                          License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>

#include "precomp.hpp"

namespace cv
{
namespace ppf_match_3d
{

48
void Pose3D::updatePose(Matx44d& NewPose)
49
{
50
  Matx33d R;
Bence Magyar's avatar
Bence Magyar committed
51

52 53
  pose = NewPose;
  poseToRT(pose, R, t);
Bence Magyar's avatar
Bence Magyar committed
54 55

  // compute the angle
56
  const double trace = cv::trace(R);
Bence Magyar's avatar
Bence Magyar committed
57 58 59 60 61 62 63

  if (fabs(trace - 3) <= EPS)
  {
    angle = 0;
  }
  else
    if (fabs(trace + 1) <= EPS)
64
    {
Bence Magyar's avatar
Bence Magyar committed
65
      angle = M_PI;
66 67
    }
    else
Bence Magyar's avatar
Bence Magyar committed
68 69 70 71 72 73
    {
      angle = ( acos((trace - 1)/2) );
    }

  // compute the quaternion
  dcmToQuat(R, q);
74 75
}

76
void Pose3D::updatePose(Matx33d& NewR, Vec3d& NewT)
77
{
78
  rtToPose(NewR, NewT, pose);
Bence Magyar's avatar
Bence Magyar committed
79 80

  // compute the angle
81
  const double trace = cv::trace(NewR);
Bence Magyar's avatar
Bence Magyar committed
82 83 84 85 86 87 88

  if (fabs(trace - 3) <= EPS)
  {
    angle = 0;
  }
  else
    if (fabs(trace + 1) <= EPS)
89
    {
Bence Magyar's avatar
Bence Magyar committed
90
      angle = M_PI;
91 92
    }
    else
Bence Magyar's avatar
Bence Magyar committed
93 94 95 96 97 98
    {
      angle = ( acos((trace - 1)/2) );
    }

  // compute the quaternion
  dcmToQuat(NewR, q);
99 100
}

101
void Pose3D::updatePoseQuat(Vec4d& Q, Vec3d& NewT)
102
{
103
  Matx33d NewR;
Bence Magyar's avatar
Bence Magyar committed
104 105

  quatToDCM(Q, NewR);
106
  q = Q;
Bence Magyar's avatar
Bence Magyar committed
107

108
  rtToPose(NewR, NewT, pose);
Bence Magyar's avatar
Bence Magyar committed
109 110

  // compute the angle
111
  const double trace = cv::trace(NewR);
Bence Magyar's avatar
Bence Magyar committed
112 113 114 115 116 117 118 119

  if (fabs(trace - 3) <= EPS)
  {
    angle = 0;
  }
  else
  {
    if (fabs(trace + 1) <= EPS)
120
    {
Bence Magyar's avatar
Bence Magyar committed
121
      angle = M_PI;
122 123
    }
    else
Bence Magyar's avatar
Bence Magyar committed
124 125 126 127
    {
      angle = ( acos((trace - 1)/2) );
    }
  }
128 129 130
}


131
void Pose3D::appendPose(Matx44d& IncrementalPose)
132
{
133 134
  Matx33d R;
  Matx44d PoseFull = IncrementalPose * this->pose;
Bence Magyar's avatar
Bence Magyar committed
135

136
  poseToRT(PoseFull, R, t);
Bence Magyar's avatar
Bence Magyar committed
137 138

  // compute the angle
139
  const double trace = cv::trace(R);
Bence Magyar's avatar
Bence Magyar committed
140 141 142 143 144 145 146

  if (fabs(trace - 3) <= EPS)
  {
    angle = 0;
  }
  else
    if (fabs(trace + 1) <= EPS)
147
    {
Bence Magyar's avatar
Bence Magyar committed
148
      angle = M_PI;
149 150
    }
    else
Bence Magyar's avatar
Bence Magyar committed
151 152 153 154 155 156 157
    {
      angle = ( acos((trace - 1)/2) );
    }

  // compute the quaternion
  dcmToQuat(R, q);

158
  pose = PoseFull;
159 160
}

161
Pose3DPtr Pose3D::clone()
162
{
163
  Ptr<Pose3D> new_pose(new Pose3D(alpha, modelIndex, numVotes));
Bence Magyar's avatar
Bence Magyar committed
164

165 166 167 168
  new_pose->pose = this->pose;
  new_pose->q = q;
  new_pose->t = t;
  new_pose->angle = angle;
Bence Magyar's avatar
Bence Magyar committed
169

170
  return new_pose;
171 172 173 174
}

void Pose3D::printPose()
{
175 176
  printf("\n-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n", (uint)this->modelIndex, (uint)this->numVotes, this->residual);
  std::cout << this->pose << std::endl;
177 178 179 180
}

int Pose3D::writePose(FILE* f)
{
Bence Magyar's avatar
Bence Magyar committed
181 182 183 184 185
  int POSE_MAGIC = 7673;
  fwrite(&POSE_MAGIC, sizeof(int), 1, f);
  fwrite(&angle, sizeof(double), 1, f);
  fwrite(&numVotes, sizeof(int), 1, f);
  fwrite(&modelIndex, sizeof(int), 1, f);
186 187 188
  fwrite(pose.val, sizeof(double)*16, 1, f);
  fwrite(t.val, sizeof(double)*3, 1, f);
  fwrite(q.val, sizeof(double)*4, 1, f);
Bence Magyar's avatar
Bence Magyar committed
189 190
  fwrite(&residual, sizeof(double), 1, f);
  return 0;
191 192 193 194
}

int Pose3D::readPose(FILE* f)
{
Bence Magyar's avatar
Bence Magyar committed
195 196 197 198 199 200 201 202
  int POSE_MAGIC = 7673, magic;

  size_t status = fread(&magic, sizeof(int), 1, f);
  if (status && magic == POSE_MAGIC)
  {
    status = fread(&angle, sizeof(double), 1, f);
    status = fread(&numVotes, sizeof(int), 1, f);
    status = fread(&modelIndex, sizeof(int), 1, f);
203 204 205
    status = fread(pose.val, sizeof(double)*16, 1, f);
    status = fread(t.val, sizeof(double)*3, 1, f);
    status = fread(q.val, sizeof(double)*4, 1, f);
Bence Magyar's avatar
Bence Magyar committed
206 207 208 209 210
    status = fread(&residual, sizeof(double), 1, f);
    return 0;
  }

  return -1;
211 212 213 214
}

int Pose3D::writePose(const std::string& FileName)
{
Bence Magyar's avatar
Bence Magyar committed
215 216 217 218 219 220 221 222 223
  FILE* f = fopen(FileName.c_str(), "wb");

  if (!f)
    return -1;

  int status = writePose(f);

  fclose(f);
  return status;
224 225 226 227
}

int Pose3D::readPose(const std::string& FileName)
{
Bence Magyar's avatar
Bence Magyar committed
228 229 230 231 232 233 234 235 236
  FILE* f = fopen(FileName.c_str(), "rb");

  if (!f)
    return -1;

  int status = readPose(f);

  fclose(f);
  return status;
237 238 239
}


240
void PoseCluster3D::addPose(Pose3DPtr newPose)
241
{
Bence Magyar's avatar
Bence Magyar committed
242 243
  poseList.push_back(newPose);
  this->numVotes += newPose->numVotes;
244 245 246 247
};

int PoseCluster3D::writePoseCluster(FILE* f)
{
Bence Magyar's avatar
Bence Magyar committed
248 249 250 251 252 253 254 255 256 257 258 259
  int POSE_CLUSTER_MAGIC_IO = 8462597;
  fwrite(&POSE_CLUSTER_MAGIC_IO, sizeof(int), 1, f);
  fwrite(&id, sizeof(int), 1, f);
  fwrite(&numVotes, sizeof(int), 1, f);

  int numPoses = (int)poseList.size();
  fwrite(&numPoses, sizeof(int), 1, f);

  for (int i=0; i<numPoses; i++)
    poseList[i]->writePose(f);

  return 0;
260 261 262 263
}

int PoseCluster3D::readPoseCluster(FILE* f)
{
Bence Magyar's avatar
Bence Magyar committed
264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281
  // The magic values are only used to check the files
  int POSE_CLUSTER_MAGIC_IO = 8462597;
  int magic=0, numPoses=0;
  size_t status;
  status = fread(&magic, sizeof(int), 1, f);

  if (!status || magic!=POSE_CLUSTER_MAGIC_IO)
    return -1;

  status = fread(&id, sizeof(int), 1, f);
  status = fread(&numVotes, sizeof(int), 1, f);
  status = fread(&numPoses, sizeof(int), 1, f);
  fclose(f);

  poseList.clear();
  poseList.resize(numPoses);
  for (size_t i=0; i<poseList.size(); i++)
  {
282
    poseList[i] = Pose3DPtr(new Pose3D());
Bence Magyar's avatar
Bence Magyar committed
283 284 285 286
    poseList[i]->readPose(f);
  }

  return 0;
287 288 289 290
}

int PoseCluster3D::writePoseCluster(const std::string& FileName)
{
Bence Magyar's avatar
Bence Magyar committed
291 292 293 294 295 296 297 298 299
  FILE* f = fopen(FileName.c_str(), "wb");

  if (!f)
    return -1;

  int status = writePoseCluster(f);

  fclose(f);
  return status;
300 301 302 303
}

int PoseCluster3D::readPoseCluster(const std::string& FileName)
{
Bence Magyar's avatar
Bence Magyar committed
304 305 306 307 308 309 310 311 312
  FILE* f = fopen(FileName.c_str(), "rb");

  if (!f)
    return -1;

  int status = readPoseCluster(f);

  fclose(f);
  return status;
313 314 315 316 317
}

} // namespace ppf_match_3d

} // namespace cv