-
Notifications
You must be signed in to change notification settings - Fork 1.9k
Python scripts to write Pose Priors into a database from a given file. #2981
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
ferreram
wants to merge
6
commits into
colmap:main
Choose a base branch
from
ferreram:feature/write_pose_priors_script
base: main
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
Changes from all commits
Commits
Show all changes
6 commits
Select commit
Hold shift + click to select a range
3110fe9
Python scripts to write Pose Priors into a database from a given file.
ferreram bb642e5
Fix ruff format error.
ferreram f5db754
Pycolmap based script for writing pose priors to database from a give…
ferreram 8102553
Pycolmap script to populate a database with pose priors from a given …
ferreram 3c1e05f
Use a context manager to open the database + use logging instead of p…
ferreram 11ec0d6
Merge branch 'main' into feature/write_pose_priors_script
ferreram File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,152 @@ | ||
| """ | ||
| Python reimplementation of the C++ incremental mapper with equivalent logic. | ||
| """ | ||
|
|
||
| import argparse | ||
| from pathlib import Path | ||
|
|
||
| import numpy as np | ||
|
|
||
| import pycolmap | ||
| from pycolmap import logging | ||
|
|
||
|
|
||
| def get_pose_prior_from_line( | ||
| line: str, | ||
| delimiter: str, | ||
| coordinate_system: int, | ||
| position_covariance: np.array, | ||
| ): | ||
| els = line.split(delimiter) | ||
|
|
||
| if len(els) == 13 and position_covariance is None: | ||
| # Get image_name, postion and covariance (in row major format) | ||
| image_name = els[0] | ||
| position = np.array([float(els[1]), float(els[2]), float(els[3])]) | ||
| covariance_values = list(map(float, els[4:13])) | ||
| position_covariance = np.array(covariance_values).reshape(3, 3) | ||
| return (image_name, position, coordinate_system, position_covariance) | ||
| elif len(els) >= 4 and position_covariance is not None: | ||
| # Get image_name, position | ||
| image_name = els[0] | ||
| position = np.array([float(els[1]), float(els[2]), float(els[3])]) | ||
| return (image_name, position, coordinate_system, position_covariance) | ||
| else: | ||
| logging.error( | ||
| "ERROR: Pose priors file lines should contain 4 or 13 elements." | ||
| ) | ||
| logging.error(f"Current line contains: {els}: # {len(els)} elements") | ||
|
|
||
| return None | ||
|
|
||
|
|
||
| def update_pose_prior_from_image_name( | ||
| colmap_db: pycolmap.Database, | ||
| image_name: str, | ||
| position: np.array, | ||
| coordinate_system: int = -1, | ||
| position_covariance: np.array = None, | ||
| ): | ||
| """ | ||
| Update the pose prior for a specific image name in the database. | ||
| If the pose prior doesn't exist, insert a new one. | ||
|
|
||
| Args: | ||
| colmap_db (pycolmap.Database): colmap database to update. | ||
| image_name (str): name of the image to update. | ||
| position (np.array): Position as a 3-element array (x, y, z). | ||
| coordinate_system (int): Coordinate system index (default: -1). | ||
| position_covariance (np.array): 3x3 position covariance matrix | ||
| (default: None). | ||
| """ | ||
| # Get image_id from image_name | ||
| if colmap_db.exists_image(image_name): | ||
| position = np.asarray(position, dtype=np.float64).reshape(3, 1) | ||
| if position_covariance is None: | ||
| position_covariance = np.full((3, 3), np.nan, dtype=np.float64) | ||
| image = colmap_db.read_image(image_name) | ||
| if colmap_db.exists_pose_prior(image.image_id): | ||
| colmap_db.update_pose_prior( | ||
| image.image_id, | ||
| pycolmap.PosePrior( | ||
| position, | ||
| position_covariance, | ||
| pycolmap.PosePriorCoordinateSystem(coordinate_system), | ||
| ), | ||
| ) | ||
| else: | ||
| colmap_db.write_pose_prior( | ||
| image.image_id, | ||
| pycolmap.PosePrior( | ||
| position, | ||
| position_covariance, | ||
| pycolmap.PosePriorCoordinateSystem(coordinate_system), | ||
| ), | ||
| ) | ||
| else: | ||
| logging.warning(f"Image at path {image_name} not found in database.") | ||
|
|
||
|
|
||
| def write_pose_priors_to_database(): | ||
| parser = argparse.ArgumentParser() | ||
| parser.add_argument("--database_path", type=str, required=True) | ||
| parser.add_argument("--pose_priors_path", type=str, required=True) | ||
| parser.add_argument("--pose_priors_delimiter", type=str, default=" ") | ||
| parser.add_argument( | ||
| "--coordinate_system", | ||
| type=int, | ||
| default=0, | ||
| help="(-1: unknwon, 0: WGS84, 1: Cartesian)", | ||
| ) | ||
| parser.add_argument( | ||
| "--use_covariance_from_pose_priors_file", | ||
| type=bool, | ||
| default=False, | ||
| help="If False, use prior_position_std options to set a \ | ||
| common covariance to the priors.", | ||
| ) | ||
| parser.add_argument("--prior_position_std_x", type=float, default=1.0) | ||
| parser.add_argument("--prior_position_std_y", type=float, default=1.0) | ||
| parser.add_argument("--prior_position_std_z", type=float, default=1.0) | ||
| args = parser.parse_args() | ||
|
|
||
| database_path = Path(args.database_path) | ||
| pose_priors_path = Path(args.pose_priors_path) | ||
|
|
||
| if not database_path.exists(): | ||
| logging.error("ERROR: database path does not exist.") | ||
| return | ||
|
|
||
| if not pose_priors_path.exists(): | ||
| logging.error("ERROR: pose priors path already does not exist.") | ||
| return | ||
|
|
||
| # Setup covariance matrix if required | ||
| position_covariance = None | ||
| if args.use_covariance_from_pose_priors_file is False: | ||
| position_covariance = np.diag( | ||
| [ | ||
| args.prior_position_std_x**2, | ||
| args.prior_position_std_y**2, | ||
| args.prior_position_std_z**2, | ||
| ] | ||
| ) | ||
|
|
||
| # Add pose priors from file. | ||
| with pycolmap.Database(database_path) as colmap_db: | ||
| with open(args.pose_priors_path) as pose_prior_file: | ||
| for line in pose_prior_file: | ||
| if line[0] == "#": | ||
| continue | ||
| pose_prior = get_pose_prior_from_line( | ||
| line, | ||
| args.pose_priors_delimiter, | ||
| args.coordinate_system, | ||
| position_covariance, | ||
| ) | ||
| if pose_prior is not None: | ||
| update_pose_prior_from_image_name(colmap_db, *pose_prior) | ||
|
|
||
|
|
||
| if __name__ == "__main__": | ||
| write_pose_priors_to_database() | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,199 @@ | ||
| # Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. | ||
| # All rights reserved. | ||
| # | ||
| # Redistribution and use in source and binary forms, with or without | ||
| # modification, are permitted provided that the following conditions are met: | ||
| # | ||
| # * Redistributions of source code must retain the above copyright | ||
| # notice, this list of conditions and the following disclaimer. | ||
| # | ||
| # * Redistributions 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. | ||
| # | ||
| # * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of | ||
| # its contributors may 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 COPYRIGHT HOLDERS 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. | ||
|
|
||
| import numpy as np | ||
| from database import COLMAPDatabase, array_to_blob | ||
|
|
||
|
|
||
| def update_pose_prior_from_image_name( | ||
| colmap_db: COLMAPDatabase, | ||
| image_name: str, | ||
| position: np.array, | ||
| coordinate_system: int = -1, | ||
| position_covariance: np.array = None, | ||
| ): | ||
| """ | ||
| Update the pose prior for a specific image name in the database. | ||
| If the pose prior doesn't exist, insert a new one. | ||
|
|
||
| Args: | ||
| colmap_db (COLMAPDatabase): colmap database to update. | ||
| image_name (str): name of the image to update. | ||
| position (np.array): Position as a 3-element array (x, y, z). | ||
| coordinate_system (int): Coordinate system index (default: -1). | ||
| position_covariance (np.array): 3x3 position covariance matrix (default: None). | ||
| """ | ||
| # Get image_id from image_name | ||
| cursor = colmap_db.execute( | ||
| "SELECT image_id FROM images WHERE name = ?", (image_name,) | ||
| ) | ||
| result = cursor.fetchone() | ||
| if result is None: | ||
| print( | ||
| "ERROR: could not retrieve image {0} in database.".format( | ||
| image_name | ||
| ) | ||
| ) | ||
| return | ||
| image_id = result[0] | ||
| position = np.asarray(position, dtype=np.float64) | ||
| if position_covariance is None: | ||
| position_covariance = np.full((3, 3), np.nan, dtype=np.float64) | ||
|
|
||
| # Check if the pose prior already exists | ||
| cursor = colmap_db.execute( | ||
| "SELECT COUNT(*) FROM pose_priors WHERE image_id = ?", (image_id,) | ||
| ) | ||
| exists = cursor.fetchone()[0] > 0 | ||
|
|
||
| if exists: | ||
| # Update the existing pose prior | ||
| colmap_db.execute( | ||
| """ | ||
| UPDATE pose_priors | ||
| SET position = ?, coordinate_system = ?, position_covariance = ? | ||
| WHERE image_id = ? | ||
| """, | ||
| ( | ||
| array_to_blob(position), | ||
| coordinate_system, | ||
| array_to_blob(position_covariance), | ||
| image_id, | ||
| ), | ||
| ) | ||
| else: | ||
| # Add a new one | ||
| colmap_db.add_pose_prior( | ||
| image_id, position, coordinate_system, position_covariance | ||
| ) | ||
|
|
||
| print( | ||
| "Pose Prior added to image #{0} ({1}): {2}".format( | ||
| image_id, image_name, position | ||
| ) | ||
| ) | ||
|
|
||
|
|
||
| def get_pose_prior_from_line( | ||
| line: str, | ||
| delimiter: str, | ||
| coordinate_system: int, | ||
| position_covariance: np.array, | ||
| ): | ||
| els = line.split(delimiter) | ||
|
|
||
| if len(els) == 13 and position_covariance is None: | ||
| # Get image_name, postion and covariance (in row major format) | ||
| image_name = els[0] | ||
| position = np.array([float(els[1]), float(els[2]), float(els[3])]) | ||
| covariance_values = list(map(float, els[4:13])) | ||
| position_covariance = np.array(covariance_values).reshape(3, 3) | ||
| return (image_name, position, coordinate_system, position_covariance) | ||
| elif len(els) >= 4 and position_covariance is not None: | ||
| # Get image_name, position | ||
| image_name = els[0] | ||
| position = np.array([float(els[1]), float(els[2]), float(els[3])]) | ||
| return (image_name, position, coordinate_system, position_covariance) | ||
| else: | ||
| print("ERROR: Pose priors file lines should contain 4 or 13 elements.") | ||
| print("Current line contains: {0}: #{1} elements".format(els, len(els))) | ||
|
|
||
| return None | ||
|
|
||
|
|
||
| def write_pose_priors_to_database(): | ||
| import argparse | ||
| import os | ||
|
|
||
| parser = argparse.ArgumentParser() | ||
| parser.add_argument("--database_path", type=str, required=True) | ||
| parser.add_argument("--pose_priors_path", type=str, required=True) | ||
| parser.add_argument("--pose_priors_delimiter", type=str, default=" ") | ||
| parser.add_argument( | ||
| "--coordinate_system", | ||
| type=int, | ||
| default=0, | ||
| help="(-1: unknwon, 0: WGS84, 1: Cartesian)", | ||
| ) | ||
| parser.add_argument( | ||
| "--use_covariance_from_pose_priors_file", | ||
| type=bool, | ||
| default=False, | ||
| help="If False, use prior_position_std options to set a common covariance to the priors.", | ||
| ) | ||
| parser.add_argument("--prior_position_std_x", type=float, default=1.0) | ||
| parser.add_argument("--prior_position_std_y", type=float, default=1.0) | ||
| parser.add_argument("--prior_position_std_z", type=float, default=1.0) | ||
| args = parser.parse_args() | ||
|
|
||
| if not os.path.exists(args.database_path): | ||
| print("ERROR: database path does not exist.") | ||
| return | ||
|
|
||
| if not os.path.exists(args.pose_priors_path): | ||
| print("ERROR: pose priors path already does not exist.") | ||
| return | ||
|
|
||
| # Open the database. | ||
| db = COLMAPDatabase.connect(args.database_path) | ||
|
|
||
| # Setup covariance matrix if required | ||
| position_covariance = None | ||
| if args.use_covariance_from_pose_priors_file is False: | ||
| position_covariance = np.diag( | ||
| [ | ||
| args.prior_position_std_x**2, | ||
| args.prior_position_std_y**2, | ||
| args.prior_position_std_z**2, | ||
| ] | ||
| ) | ||
|
|
||
| # Add pose priors from file. | ||
| pose_prior_file = open(args.pose_priors_path, "r") | ||
| for line in pose_prior_file: | ||
| if line[0] == "#": | ||
| continue | ||
| pose_prior = get_pose_prior_from_line( | ||
| line, | ||
| args.pose_priors_delimiter, | ||
| args.coordinate_system, | ||
| position_covariance, | ||
| ) | ||
| if pose_prior is not None: | ||
| update_pose_prior_from_image_name(db, *pose_prior) | ||
|
|
||
| # Commit the data to the file. | ||
| db.commit() | ||
|
|
||
| # Close database. | ||
| db.close() | ||
|
|
||
|
|
||
| if __name__ == "__main__": | ||
| write_pose_priors_to_database() |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I guess this is left over from copying another sample file?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Indeed, I forgot to remove this header comment.