parameters: (self, W)
return_statement: return problem
docstring: Build the optimisation problem
docstring_summary: Build the optimisation problem
docstring_tokens: Build the optimisation problem
function: def __buildOptimizationProblem(self, W):
          """Build the optimisation problem"""
          problem = inc.CalibrationOptimizationProblem()
  
          # Initialize all design variables.
          self.__initPoseDesignVariables(problem)
  
          #####
          ## build error terms and add to problem
  
          # store all frames
          self.__frames = []
          self.__reprojection_errors = []
  
          # This code assumes that the order of the landmarks in the observations
          # is invariant across all observations. At least for the chessboards it is true.
  
          #####
          # add all the landmarks once
          landmarks = []
          landmarks_expr = {}
          keypoint_ids0 = self.__observations[0].getCornersIdx()
          for idx, landmark in enumerate(self.__observations[0].getCornersTargetFrame()):
              # design variable for landmark
              landmark_w_dv = aopt.HomogeneousPointDv(sm.toHomogeneous(landmark))
              landmark_w_dv.setActive(self.__config.estimateParameters['landmarks']);
              landmarks.append(landmark_w_dv)
              landmarks_expr[keypoint_ids0[idx]] = landmark_w_dv.toExpression()
              problem.addDesignVariable(landmark_w_dv, CALIBRATION_GROUP_ID)
  
          #####
          # activate design variables
          self.__camera_dv.setActive(
              self.__config.estimateParameters['intrinsics'],
              self.__config.estimateParameters['distortion'],
              self.__config.estimateParameters['shutter']
          )
  
          #####
          # Add design variables
  
          # add the camera design variables last for optimal sparsity patterns
          problem.addDesignVariable(self.__camera_dv.shutterDesignVariable(), CALIBRATION_GROUP_ID)
          problem.addDesignVariable(self.__camera_dv.projectionDesignVariable(), CALIBRATION_GROUP_ID)
          problem.addDesignVariable(self.__camera_dv.distortionDesignVariable(), CALIBRATION_GROUP_ID)
  
          #####
          # Regularization term / motion prior
          motionError = asp.BSplineMotionError(self.__poseSpline_dv, W)
          problem.addErrorTerm(motionError)
  
          #####
          # add a reprojection error for every corner of each observation
          for frameid, observation in enumerate(self.__observations):
              # only process successful observations of a pattern
              if (observation.hasSuccessfulObservation()):
                  # add a frame
                  frame = self.__cameraModelFactory.frameType()
                  frame.setGeometry(self.__camera)
                  frame.setTime(observation.time())
                  self.__frames.append(frame)
  
                  #####
                  # add an error term for every observed corner
                  for index, point in enumerate(observation.getCornersImageFrame()):
  
                      # keypoint time offset by line delay as expression type
                      keypoint_time = self.__camera_dv.keypointTime(frame.time(), point)
  
                      # from target to world transformation.
                      T_w_t = self.__poseSpline_dv.transformationAtTime(
                          keypoint_time,
                          self.__config.timeOffsetConstantSparsityPattern,
                          self.__config.timeOffsetConstantSparsityPattern
                      )
                      T_t_w = T_w_t.inverse()
  
                      # we only have the the first image's design variables
                      # so any landmark that is not in that frame won't be in the problem
                      # thus we must skip those measurements that are of a keypoint that isn't visible
                      keypoint_ids = observation.getCornersIdx()
                      if not np.any(keypoint_ids[index]==keypoint_ids0):
                         sm.logWarn("landmark {0} in frame {1} not in first frame".format(keypoint_ids[index],frameid))
                         continue
  
                      # transform target point to camera frame
                      p_t = T_t_w * landmarks_expr[keypoint_ids[index]]
  
                      # create the keypoint
                      keypoint_index = frame.numKeypoints()
                      keypoint = acv.Keypoint2()
                      keypoint.setMeasurement(point)
                      inverseFeatureCovariance = self.__config.inverseFeatureCovariance;
                      keypoint.setInverseMeasurementCovariance(np.eye(len(point)) * inverseFeatureCovariance)
                      keypoint.setLandmarkId(keypoint_index)
                      frame.addKeypoint(keypoint)
  
                      # create reprojection error
                      reprojection_error = self.__buildErrorTerm(
                          frame,
                          keypoint_index,
                          p_t,
                          self.__camera_dv,
                          self.__poseSpline_dv
                      )
                      self.__reprojection_errors.append(reprojection_error)
                      problem.addErrorTerm(reprojection_error)
  
          return problem
function_tokens: def __buildOptimizationProblem ( self , W ) : problem = inc . CalibrationOptimizationProblem ( ) # Initialize all design variables. self . __initPoseDesignVariables ( problem ) ##### ## build error terms and add to problem # store all frames self . __frames = [ ] self . __reprojection_errors = [ ] # This code assumes that the order of the landmarks in the observations # is invariant across all observations. At least for the chessboards it is true. ##### # add all the landmarks once landmarks = [ ] landmarks_expr = { } keypoint_ids0 = self . __observations [ 0 ] . getCornersIdx ( ) for idx , landmark in enumerate ( self . __observations [ 0 ] . getCornersTargetFrame ( ) ) : # design variable for landmark landmark_w_dv = aopt . HomogeneousPointDv ( sm . toHomogeneous ( landmark ) ) landmark_w_dv . setActive ( self . __config . estimateParameters [ 'landmarks' ] ) landmarks . append ( landmark_w_dv ) landmarks_expr [ keypoint_ids0 [ idx ] ] = landmark_w_dv . toExpression ( ) problem . addDesignVariable ( landmark_w_dv , CALIBRATION_GROUP_ID ) ##### # activate design variables self . __camera_dv . setActive ( self . __config . estimateParameters [ 'intrinsics' ] , self . __config . estimateParameters [ 'distortion' ] , self . __config . estimateParameters [ 'shutter' ] ) ##### # Add design variables # add the camera design variables last for optimal sparsity patterns problem . addDesignVariable ( self . __camera_dv . shutterDesignVariable ( ) , CALIBRATION_GROUP_ID ) problem . addDesignVariable ( self . __camera_dv . projectionDesignVariable ( ) , CALIBRATION_GROUP_ID ) problem . addDesignVariable ( self . __camera_dv . distortionDesignVariable ( ) , CALIBRATION_GROUP_ID ) ##### # Regularization term / motion prior motionError = asp . BSplineMotionError ( self . __poseSpline_dv , W ) problem . addErrorTerm ( motionError ) ##### # add a reprojection error for every corner of each observation for frameid , observation in enumerate ( self . __observations ) : # only process successful observations of a pattern if ( observation . hasSuccessfulObservation ( ) ) : # add a frame frame = self . __cameraModelFactory . frameType ( ) frame . setGeometry ( self . __camera ) frame . setTime ( observation . time ( ) ) self . __frames . append ( frame ) ##### # add an error term for every observed corner for index , point in enumerate ( observation . getCornersImageFrame ( ) ) : # keypoint time offset by line delay as expression type keypoint_time = self . __camera_dv . keypointTime ( frame . time ( ) , point ) # from target to world transformation. T_w_t = self . __poseSpline_dv . transformationAtTime ( keypoint_time , self . __config . timeOffsetConstantSparsityPattern , self . __config . timeOffsetConstantSparsityPattern ) T_t_w = T_w_t . inverse ( ) # we only have the the first image's design variables # so any landmark that is not in that frame won't be in the problem # thus we must skip those measurements that are of a keypoint that isn't visible keypoint_ids = observation . getCornersIdx ( ) if not np . any ( keypoint_ids [ index ] == keypoint_ids0 ) : sm . logWarn ( "landmark {0} in frame {1} not in first frame" . format ( keypoint_ids [ index ] , frameid ) ) continue # transform target point to camera frame p_t = T_t_w * landmarks_expr [ keypoint_ids [ index ] ] # create the keypoint keypoint_index = frame . numKeypoints ( ) keypoint = acv . Keypoint2 ( ) keypoint . setMeasurement ( point ) inverseFeatureCovariance = self . __config . inverseFeatureCovariance keypoint . setInverseMeasurementCovariance ( np . eye ( len ( point ) ) * inverseFeatureCovariance ) keypoint . setLandmarkId ( keypoint_index ) frame . addKeypoint ( keypoint ) # create reprojection error reprojection_error = self . __buildErrorTerm ( frame , keypoint_index , p_t , self . __camera_dv , self . __poseSpline_dv ) self . __reprojection_errors . append ( reprojection_error ) problem . addErrorTerm ( reprojection_error ) return problem